記事のディレクトリ
3軸ロボットアームの制御原理
私が書いたブログを参照してください：
アルゴリズム-3軸ロボットアームの運動式を逆に解いて、運動座標を取得します
3軸ロボットアームの配線
| ロボットアームサーボ | PYB |
|---|---|
| 軸1 | X1 |
| 軸2 | X2 |
| 軸3 | X3 |
注：サーボの5VとGNDには、十分な量の5Vを外部から供給することが望ましいです。オンボードのV +インターフェース電源は5V未満であるため、サーボが回転しなくなる可能性があります。！にぎやか。
3軸ロボットアーム制御
ブロガーはメインコントロールとしてPYBを使用し、直接コントロールにはサーボライブラリを使用します。
垂直座標系制御
平面内の座標系の移動計算を完了することができるのは2つの軸だけです。
- 座標計算機能
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)