Article Directory
Control principle of three-axis robotic arm
Refer to the blog I wrote:
Algorithm-Inversely solve the three-axis robotic arm motion formula to obtain the motion coordinates
Three-axis robotic arm wiring
| Robot arm servo | PYB |
|---|---|
| Axis 1 | X1 |
| Axis 2 | X2 |
| Axis 3 | X3 |
Note : The 5V and GND of the servo should preferably be externally supplied with a sufficient amount of 5V. The on-board V+ interface power supply is less than 5V, which may cause the servo to fail to rotate! ! Only buzzing.
Three-axis robotic arm control
The blogger uses PYB as the main control and uses the Servo library for direct control.
Vertical coordinate system control
Only two axes can complete the movement calculation of the coordinate system in the plane.
- Coordinate calculation function
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
- Control code
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)
Stereo space coordinate system control
Just add a Z axis . Judge for yourself
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)