目次
2019b/Mission1を参照。
x軸とy軸、ペンの上げ下げにそれぞれ1つずつモーターを配置したロボットを制作した。なぜなら、プログラムが簡単そうだったからだ。
タイヤがあると誤差が大きくなると考えたので、物理的にモーターとつながっている構造にしたいと思った。そこで、スライダクランク機構を用いて x軸 , y軸 の動きを作ることにした。
x軸 , y軸はこのようになった。2つの軸には鏡で映しただけで同じ構造のものを用いた。
ペンの取付部のパーツはこのようにした。
ペンの取り付けパーツはそれぞれの軸の上に乗っかっているだけである。軸に垂直な方向にしか動かないように軸上のスリットのところに置く。
ペンの上げ下げは、以下の写真のモーターでペン取り付けパーツの端を押すことで実現させる。
今回は、サーボモーターの機能を使用した。
#define SPEED 10 #define P 40 #define I 40 #define D 90 #define gf(x,y,z) gt(x-log_x,y-log_y,z);log_x=x;log_y=y; #define pu OnFwd(OUT_A,20);Wait(500);Off(OUT_A); //ペン上げ #define pd OnFwd(OUT_A,-SPEED);Wait(300);Off(OUT_A);Wait(600); //ペン下げ #define gh gf(0,0,1); //ホームポジション( 0 , 0 )に戻る sub gt(float x,float y,float z) //(行き先の座標 , 繰り返しの回数) { const float axis = 100.0; //軸 const int rep = z; //繰り返し回数 const float rotation = 180.0; //軸を端から端まで動かすのに必要な角度 const float correction = 0.5; //補正 float angle_x = ((rotation/axis)*x)/rep + sign(x)*correction; float angle_y = ((rotation/axis)*y)/rep + sign(y)*correction; repeat(rep) { //RotateMotor(OUT_B,SPEED,angle_x); RotateMotorPID(OUT_B,SPEED,angle_x,P,I,D); //RotateMotor(OUT_C,SPEED,angle_y); RotateMotorPID(OUT_C,SPEED,angle_y,P,I,D); } Wait(400); } task main() { ResetTachoCount(OUT_BC); float log_x = 0; float log_y = 0; //輪郭 pu; gf(0.0,100.0,2); pd; gf(0.0,35.0,1); gf(35.0,0.0,30); gf(45.0,0.0,1); gf(100.0,35.0,30); gf(100.0,100.0,1); gf(0.0,100.0,1); pu; //メガネ gf(30.0,50.0,2); pd; gf(50.0,50.0,1); gf(50.0,40.0,1); gf(25.0,40.0,1); gf(25.0,60.0,1); pu; gf(50.0,45.0,1); pd; gf(60.0,45.0,1); pu; gf(60.0,55.0,2); pd; gf(80.0,55.0,1); gf(80.0,40.0,1); gf(52.0,40.0,1); gf(52.0,55.0,1); pu; //鼻 gf(45.0,35.0,2); pd; gf(40.0,25.0,10); gf(50.0,28.0,1); pu; //口 gf(30.0,15.0,2); pd; gf(60.0,15.0,2); pu; gh; }
マクロを使用して、pu = pen up , pd = pen down として指定した。
#define SPEED 10 #define pu OnFwd(OUT_A,20);Wait(500);Off(OUT_A); #define pd OnFwd(OUT_A,-SPEED);Wait(300);Off(OUT_A);Wait(600);
指定した座標と今いる座標を直線で結んでくれるような関数を "define" と "sub" を用いて作った。
#define SPEED 10 #define P 40 #define I 40 #define D 90 #define gf(x,y,z) gt(x-log_x,y-log_y,z);log_x=x;log_y=y;
sub gt(float x,float y,float z) //(行き先の座標 , 繰り返しの回数) { const float axis = 100.0; //軸 const int rep = z; //繰り返し回数 const float rotation = 180.0; //軸を端から端まで動かすのに必要な角度 const float correction = 0.5; //補正 float angle_x = ((rotation/axis)*x)/rep + sign(x)*correction; float angle_y = ((rotation/axis)*y)/rep + sign(y)*correction; repeat(rep) { //RotateMotor(OUT_B,SPEED,angle_x); RotateMotorPID(OUT_B,SPEED,angle_x,P,I,D); //RotateMotor(OUT_C,SPEED,angle_y); RotateMotorPID(OUT_C,SPEED,angle_y,P,I,D); } Wait(400); }
task main() { float log_x = 0; float log_y = 0; }
これにより、以下のように書くことで、指定した座標まで動かすことができる。"sub"だけでは相対的な座標で動いてしまうので、"log_x","log_y"という変数を用意して、直前に指定された座標を記憶しておくことで、絶対的な座標として動かすことを可能にした。ここでは、x軸 y軸ともに 100 目盛りとして計算した。
gf(移動先の x座標 , 移動先の y座標 , 繰り返し回数)
NXTでは、2つのモーターを同時に違う出力で動かすことができない。そのため、斜めの線をひくことができない。そこで、階段を書くような感じで、x軸 y軸方向に少しずつ動かして斜めの線を書くようにした。ここでいう階段の数というのが関数の引数の中にある「繰り返し回数」である。繰り返し回数を増やすほどなめらかな線を描ける。
今回は、"RotateMotor"よりも精度がいいらしい "RotateMotorPID" を使用した。"P , I , D"はこれに必要な値である。
また、今回制作したロボットは関節部分にぐらつきがあったので、そこから生まれる誤差を埋めるために「補正」として回転角に"0.5"度追加した。
通ってほしい座標を指定しているだけなのでスッキリしている。ペンの抵抗によってずれてしまうことがあったが、調整も座標をいじるだけで済んだ。
3番目の引数は x軸 , y軸 に平行な場合は"1 or 2"で、その他の場合は"10"以上になっている。
task main() { ResetTachoCount(OUT_BC); float log_x = 0; float log_y = 0; //輪郭 pu; gf(0.0,100.0,2); pd; gf(0.0,35.0,1); gf(35.0,0.0,30); gf(45.0,0.0,1); gf(100.0,35.0,30); gf(100.0,100.0,1); gf(0.0,100.0,1); pu; //メガネ gf(30.0,50.0,2); pd; gf(50.0,50.0,1); gf(50.0,40.0,1); gf(25.0,40.0,1); gf(25.0,60.0,1); pu; gf(50.0,45.0,1); pd; gf(60.0,45.0,1); pu; gf(60.0,55.0,2); pd; gf(80.0,55.0,1); gf(80.0,40.0,1); gf(52.0,40.0,1); gf(52.0,55.0,1); pu; //鼻 gf(45.0,35.0,2); pd; gf(40.0,25.0,10); gf(50.0,28.0,1); pu; //口 gf(30.0,15.0,2); pd; gf(60.0,15.0,2); pu; gh; }
メガネのフレームがうまくかけた。しかし、髪の毛は難しくて諦めた。
今回作った関数の中に、モーターの回転角を計算する式を組み込んだが、それが間違っていた。モーターの回転角とアームが左右に動く大きさは比例していると考えて式を立てていた。実際には、2つは比例関係にはなく、三角関数を使用しないと正しい結果は出てこないことに後になって気づいた。今回使用したプログラムは座標の調節によって顔を描くことができていた。
実際には、
であり、関数は、
sub gt(float x,float y,float z) //(行き先の座標 , 繰り返しの回数) { const float axis = 100.0; //軸 const float arm_1 = r; //アーム1の長さ const float arm_2 = R; //アーム2の長さ const int rep = z; //繰り返し回数 const float length = l; //アームの動く距離 const float correction = 0.5; //補正 float X = l/axis*x; float Y = l/axis*y; float angle_x = (acos(((arm_1^2+arm_2^2-X)^2-(arm_2-arm_1)^2)/(2*arm_1*(arm_1+arm_2-X))))/rep + sign(x)*correction; float angle_y = (acos(((arm_1^2+arm_2^2-Y)^2-(arm_2-arm_1)^2)/(2*arm_1*(arm_1+arm_2-Y))))/rep + sign(y)*correction; repeat(rep) { //RotateMotor(OUT_B,SPEED,angle_x); RotateMotorPID(OUT_B,SPEED,angle_x,P,I,D); //RotateMotor(OUT_C,SPEED,angle_y); RotateMotorPID(OUT_C,SPEED,angle_y,P,I,D); } Wait(400); }
とすべきであった。