三轴机械臂的控制原理
参考我写的博客：
三轴机械臂接线
| 机械臂舵机 | PYB |
|---|---|
| 轴1 | X1 |
| 轴2 | X2 |
| 轴3 | X3 |
注意：舵机的5V和GND最好外接足额的5V供电，板载的V+接口供电不足5V，可能会导致舵机无法转动！！只会嗡嗡响。
三轴机械臂控制
博主采用PYB作为主控，利用Servo库进行直接控制。
垂直坐标系控制
只用两个轴即可完成平面内坐标系的移动解算。
- 坐标解算函数
def arm_x_y_count(x2, y2, h1 = 10.98, h2=15.66):
angel1, angel2 = 0
angel1 = math.acos((x2**2 + y2**2 + h1**2 - h2**2)/(2*h1* math.sqrt(x2**2 + y2**2)) + math.atan(y2/x2))
angel2 = math.acos((x2 - h1*math.cos(angel1))/h2) - angel1
angel1 = math.degrees(angel1)
angel2 = math.degrees(angel2)
return angel1, angel2
- 控制代码
from pyb import Pin, Timer, Servo
import math
import time
def arm_x_y_count(x2, y2, h1 = 10.98, h2=15.66):
angel1, angel2 = 0
angel1 = math.acos((x2**2 + y2**2 + h1**2 - h2**2)/(2*h1* math.sqrt(x2**2 + y2**2)) + math.atan(y2/x2))
angel2 = math.acos((x2 - h1*math.cos(angel1))/h2) - angel1
angel1 = math.degrees(angel1)
angel2 = math.degrees(angel2)
return angel1, angel2
if __name__ == "__main__":
p1 = Pin("X1", Pin.OUT_PP)
p2 = Pin("X2", Pin.OUT_PP)
s1 = Servo(1)
s2 = Servo(2)
angel1, angel2 = arm_x_y_count(16, 0) #这里为要去的点的(x,y)坐标
s1.angle(angel1, 1500)
s2.angle(angel2, 1500)
立体空间坐标系控制
只需加一个Z轴即可。自己判断一下自己的
from pyb import Pin, Timer, Servo
import math
import time
def arm_x_y_count(x2, y2, h1 = 10.98, h2=15.66):
angel1, angel2 = 0
angel1 = math.acos((x2**2 + y2**2 + h1**2 - h2**2)/(2*h1* math.sqrt(x2**2 + y2**2)) + math.atan(y2/x2))
angel2 = math.acos((x2 - h1*math.cos(angel1))/h2) - angel1
angel1 = math.degrees(angel1)
angel2 = math.degrees(angel2)
return angel1, angel2
if __name__ == "__main__":
angel3 = 0
p1 = Pin("X1", Pin.OUT_PP)
p2 = Pin("X2", Pin.OUT_PP)
p3 = Pin("X3", Pin.OUT_PP)
s1 = Servo(1)
s2 = Servo(2)
s3 = Servo(3)
angel1, angel2 = arm_x_y_count(16, 0) #这里为要去的点的(x,y)坐标
angle3 = math.degrees(angel3) #需要按照自己的舵机更改angle3
s1.angle(angel1, 1500)
s2.angle(angel2, 1500)
s3.angle(angel3, 1500)