Processing で制御 ーロボットアーム自作への道3ー
お父ちゃん
2019.12.12
Home > Advent Calendar >
0
0
さて前回は逆運動学によって、ロボットアームの座標からサーボの角度を算出して制御を楽しみました。
そうなるとコントローラでリアルタイムに動かしたくなるのがヒトというものです。
ここではProcessingを用いてマウスでのPC上のお絵描きを同時にロボットアームでもしてしまいます。
目次
可動範囲拡張
前回よりもアームの可動範囲を拡張したく、3つ目のサーボの0°基準を変更しました($θ_3$)。
アーム$L_2$に対してアーム$L_3$が直角になる地点を0°としていたのですが、可動範囲が限られてしまったため、90°にシフトしました。
前回導出した3個目のサーボの角度$θ_3$に90°足すだけです。
$$θ_3 = \sin^{-1} \left(\frac{{l_d}^2-{L_2}^2 – {L_3}^2}{2{L_2}{L_3}} \right) + 90°$$
構成
Processingのウィンドウ上の座標をArduinoに送信してロボットアームの平面状の座標(x, y)として制御します。
マウスをクリックするとアームについたマジックペンの先が地面に接地し(z = 0)、ドラッグで線や絵を描けます。マウスがクリックされていない時はz = 20として、ペン先を浮かせます。
動作
まずは早速 動作を見てください！
Processingのウィンドウ上の線と同じように、ロボットアームも線を描きます。
まずは長女ちゃん
が動作テストしてくれました。
長女ちゃんによる動作テスト#ロボットアーム pic.twitter.com/KYa9CKp6zk
— HomeMadeGarbage (@H0meMadeGarbage) December 9, 2019
次にお母ちゃん
がノリノリでミッフィーちゃんを描いてくれました。。。
怖い#ミッフィー pic.twitter.com/eibTWdpj56
— HomeMadeGarbage (@H0meMadeGarbage) December 9, 2019
やはりまだまだ精度は完ぺきではありません。
ミッフィーちゃんの顔怖い(´；ω；`)。。。
最後は真打ちお父ちゃん
です。ロボットアームの精度を考慮し、大きめに描画し、マジックペンと紙の摩擦もふまえてゆっくり線を引いています。
processingで制御#ロボットアーム pic.twitter.com/jyL0BustsB
— HomeMadeGarbage (@H0meMadeGarbage) December 8, 2019
どうでしょうか？なかなかのものではないでしょうか？
ガッツリ ロボットアームに忖度すれば使えないレベルではありません。まさに愛。
Processingコード
400 × 520のウィンドウを表示して、ウィンドウ上のカーソルをマウスで動かして、座標(x, y)をPCにつながったArduinoに送信します。座標の数値は1/4に圧縮して送信しています。
マウスがクリックされていないときはz = 20を、クリックされているときはz = 0を送信します。
マウスをクリックしてドラッグするとウィンドウ上に線が描けます。キーボード”q”で消去します。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 |
import processing.serial.*; Serial myPort; // シリアルポート float i; float j; int x = 0, y = 200, z = 20; void setup() { size(400, 520); strokeWeight(10); background(200); myPort = new Serial(this, Serial.list()[0], 9600); } void draw() { if (mousePressed == true){ line(mouseX, mouseY, pmouseX, pmouseY); z = 0; }else{ z = 20; } x = pmouseX/4; y = pmouseY/4; println(x + ", " + y + ", " + z); myPort.write('P'); myPort.write(x); myPort.write(y); myPort.write(z); //消去 if ((keyPressed == true) && ((key == 'q'))) { background(200); } } |
参考
Arduinoコード
Processingからの座標データを受けて、逆運動学でアームをリアルタイムに制御します。
Processingのウィンドウの座標とロボットアームの平面座標にオフセットがありますので、受信後に変換処理をしています。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 |
#include <Servo.h> Servo myservo1, myservo2, myservo3; float th1=90.0,th2=90.0,th3=90.0; float phi,ld,l; float L1=92,L2=137,L3=158; int x,y,z; void setup() { Serial.begin(9600); myservo1.attach(9, 500, 2420); //MG995 myservo2.attach(10, 820, 2140); //DS3115 myservo3.attach(11, 820, 2140); //DS3115 set_servo(); ik(0, 170, 230); //初期姿勢 } void loop() { if (Serial.available() >= 4) { if ( Serial.read() == 'P' ) { x = Serial.read()-50; y = 240 - Serial.read(); z = Serial.read(); ik(x, y, z); } } } void ik(float x,float y,float z){ th1=atan2(y,x); l=sqrt(x*x + y*y); ld=sqrt(l*l + (z-L1)*(z-L1)); phi=atan2((z-L1),l); th2=phi + acos((ld*ld+L2*L2-L3*L3)/(2.0*ld*L2)); th3=asin((ld*ld-L2*L2-L3*L3)/(2.0*L2*L3)) + M_PI /2.0; th1=th1*180.0/M_PI ; th2=th2*180.0/M_PI ; th3=th3*180.0/M_PI ; set_servo(); } void set_servo(){ myservo1.write(th1); myservo2.write(th2); myservo3.write(th3); } |
おわりに
Processingのおかげで簡単にリアルタイム制御ができてしまいました。
ちょっと精度が悪いので勉強進めます。
それではまた
