Micropython——基于单片机的三轴机械臂的控制

三轴机械臂的控制原理

参考我写的博客：

Algorithm——逆解算三轴机械臂运动公式得到运动坐标

三轴机械臂接线

机械臂舵机	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)