麦克纳姆轮画圆首先要有一个速度控制转换为每个轮子转速的函数：move(左右速度，前后速度，转弯速度）
def move(self, LR, FB, turn):
move_sum = abs(FB) + abs(LR) + abs(turn)
if move_sum >= 100:
k = 100 / move_sum
FB = FB * k
LR = LR * k
turn = turn * k
self.MX_motorControl(self.MX_digitalIO_1, 10, FB + LR * 1 + turn, 0)
self.MX_motorControl(self.MX_digitalIO_2, 10, FB + LR * -1 + turn * -1, 0)
self.MX_motorControl(self.MX_digitalIO_3, 10, FB + LR * -1 + turn, 0)
self.MX_motorControl(self.MX_digitalIO_4, 10, FB + LR * 1 + turn * -1, 0)
然后麦克纳姆轮小车要有一个能够转到自身坐标系下对应位置x,y的移动函数：move_to(x,y)
def move_to(self, x, y):
t = 0
distance = math.sqrt(x ** 2 + y ** 2)
time_spent = distance / self.moveSpeed
lr_speed = int(x * self.moveSpeed / distance)
fb_speed = int(y * self.moveSpeed / distance)
while t < time_spent:
self.move(lr_speed, fb_speed, 0)
time.sleep(0.1)
t = t + 0.1
最后使用move_to函数绘制出圆形
def 画圆(self, r):
angle = 0
while True:
self.m.move_to(x=math.sin(angle) * r, y=math.cos(angle) * r)
angle += 0.1
if angle > math.pi * 2:
break
要画圆需要舵机配合，舵机程序rotation.py不展开了，查看附件自动运行麦轮小车.zip
使用画圆函数可以绘制出数字100
from motor import Motor
import math
from time import sleep
from draw import 画图
from Rotation import Rotation
def 画直线(x,y):
servo.specifyRotation(down) # 放下舵机的笔
m.move_to(x, y)
m.MX_motorLockAll()
servo.specifyRotation(up) # 抬起
def 画圆(r):
servo.specifyRotation(down) # 放下舵机的笔
draw.画圆(r)
servo.specifyRotation(up) # 抬起
if __name__ == "__main__":
# global up, down, m, servo, draw
up = 60
down = 90
m = Motor()
servo = Rotation(18, 0, 90, up) # 实例servo舵机引脚18，转动范围0-90度,初始角度up
servo.setup() # 初始化servo舵机
sleep(1)
draw = 画图()
画直线(x=0, y=-20)
m.move_to(5,5)
画圆(1)
m.move_to(20,0)
画圆(1)
m.MX_motorUnlockAll()
servo.cleanup()
详细资料请查看附件
