![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
#author("2020-02-15T20:53:05+09:00","yamaji","yamaji") #author("2020-02-15T20:56:34+09:00","yamaji","yamaji") 目次 #contents *課題3 [#fa13243d] 課題3は簡単にまとめると缶の上に乗っているボールを別の缶の上に乗せるというものだった。詳しくは[[2019b/Mission3]]を参照。 *ロボットの説明 [#k0336c96] 私たちは効率を重視し、3つのボールを一度に運ぶロボットを作った。 **ボールを確保し、貯めておく機構 [#ccbd3067] #ref(2019b/Member/yamaji/Mission3/枠.jpg,50%,ゴム枠) ボールを確保する機構は上の写真のように、ゴムを使って組み立てた。（以後この機構をゴム枠と言う。） #ref(2019b/Member/yamaji/Mission3/キャッチ1.JPG,50%,ボールの保存場所) ボールを確保するにはゴム枠をボールに押し付ける。すると、ボールがゴムを通り抜け、上に乗るため、ボールが確保できる。この時、ゴムの柔軟性により、ゴム枠が多少缶からずれてしまってもボールを確保できる。 ボールを確保したら、そのままゴム枠を上に上げることで、ゴム枠の後ろに連結しているレールの上にボールが貯まる仕組みになっている。 **ボールを缶の上に乗せる機構 [#sf44c204] &ref(2019b/Member/yamaji/Mission3/アーム.jpg,50%,アーム); &ref(2019b/Member/yamaji/Mission3/置く.jpg,50%,置く仕組み); ボールを缶に乗せる時のポイントとなるのは左の写真のアームである。このアームでボールを抑え、ゴム枠を上に上げることでボールがゴムを通り抜け缶の上に乗る仕組みとなっている。このとき、２つ工夫した点がある。 1つは下に示す棒についてである。少し見えにくいが、この棒があることによって、アームを左に傾けるとレールの傾斜が無くなり、アームを右に傾けるとレールに傾斜が付きボールがゴム枠の方へ転がるようになっている。 #ref(2019b/Member/yamaji/Mission3/棒の役割.jpg,50%,アーム) ２つ目は、ボールを缶に乗せるときにゴム枠にボールが一つずつ乗るように、ボールの運び役かつ、次のボールのストッパーの役割を果たす突起である。この突起はゴム枠と連動しているため、写真のように、ゴム枠が床と水平になると出てきて、ゴム枠が垂直になると隠れるようになっている。したがって、ボールを缶に置く時には、下の写真のようにボールを隔てることができる。 #ref(2019b/Member/yamaji/Mission3/つっかえ.jpg,50%,アーム) **センサ [#j26996ce] 缶の位置の把握には超音波センサを使った。超音波センサは付ける位置が重要になるので悩んでいたが、ゴム枠とアームを下の写真のようにうまく利用すると、良い位置に付けることができた。また、写真の位置を超音波センサの初期位置とした。 #ref(2019b/Member/yamaji/Mission3/初期位置1.jpg,50%,超音波センサ) その他のセンサとしては、光センサをライントレースのために2つ使った。 **全体 [#vb9f84f1] &ref(2019b/Member/yamaji/Mission3/全体像1.jpg,50%,全体像); &ref(2019b/Member/yamaji/Mission3/コード.jpg,50%,キャスター); 最終的な全体像はこのようになった。初めは写真のキャスターの部分が、プラスチックの部品で支えるだけになっていたが、実際に走らせてみると摩擦が大きく、モータに負担が掛かっていそうだったのでキャスターに変えた。 また、初めはボールを掴む機構と缶に置く機構を別々に逆方向に付けていたが、同一方向に付けることで、旋回などの手間を省き、進行方向のライントレースだけで全てを行えるようにした。 *プログラムの説明 [#kf274dc0] **旋回のプログラム [#hec2e5be] その場で旋回するのはよく使うため、関数にしておいた。 void turnAng(long ang) //ang度の反時計回りの旋回 { const float diameter = 5.45; //タイヤの直径 const float track = 11.05; //タイヤ間の幅 const float pi = 3.1415; //円周率 long angle; angle = (track/diameter)*ang; RotateMotorEx(OUT_AC,spd,angle,100*sign(ang),true,true); } **前後移動のプログラム [#m2a6d8ad] 私たちのロボットは構造上、缶からボールを取ったあとも超音波センサが缶に反応してしまうため、取ったあとの缶をどかすために指定距離だけ前後に動くプログラムを作った。 void fwd_back(float d) //距離ｄcm前進 { const float diameter = 5.45; //タイヤの直径 const float track = 11.05; //タイヤ間の幅 const float pi = 3.1415; //円周率 long degree; degree = d/(diameter*pi)*360.0; RotateMotorEx(OUT_BC,spd,degree,0,true,true); } **ライントレースのプログラム [#r4ace34a] ライントレースでは光センサを2つ使って制御した。 #define spd 35 //ロボットのスピード #define white 52 //白 #define black 33 //黒 #define SIGNALON 11 #define SIGNALOFF 10 上のように閾値などを設定した。 void follow_line(long tmin, long tmax) //ライントレース(右モータにC、左モータにB) { long t_start; SetSensorLight(S1); SetSensorLight(S2); t_start = CurrentTick(); //この関数が動き始めた時間 float Sl_l, Sl_r; //光センサーの値 Sl_l = SENSOR_1; Sl_r = SENSOR_2; int msg; //メッセージを受け取ったあとの保存場所として定義した //スタートからtmin以下ではライントレースのみ while (CurrentTick() - t_start < tmin) { long t_start; Sl_l = SENSOR_1; Sl_r = SENSOR_2; ReceiveRemoteNumber(MAILBOX2,true,msg); //メールボックス２にメッセージを受け取ったらライントレースを止めてマスター側のメールボックス３にメッセージを返す if (msg == SIGNALON) { SendResponseNumber(MAILBOX3,SIGNALON); break; } else if ((Sl_l >= white) && (Sl_r >= white) || (Sl_l <= black)&&(Sl_r <= black)) //左右のセンサの値が両方白い、または黒い場合、直進する { OnFwdSync(OUT_BC,spd,0); } else //比例制御をする（詳しくは下で説明する） { OnFwd(OUT_B,(spd / (white - black)) * (Sl_l - black) + sign(Sl_l - black) * 20); OnFwd(OUT_C,(spd / (white - black)) * (Sl_r - black) + sign(Sl_r - black) * 20); } } Off(OUT_BC); //スタートからtmax以上では停止 while (CurrentTick() - t_start < tmax) { Sl_l = SENSOR_1; Sl_r = SENSOR_2; ReceiveRemoteNumber(MAILBOX2,true,msg); //メールボックス２にメッセージを受け取ったらライントレースを止めてマスター側のメールボックス３にメッセージを返す if (msg == SIGNALON) { Off(OUT_BC); SendResponseNumber(MAILBOX3,SIGNALON); break; } else if ((Sl_l >= white) && (Sl_r >= white)) //左右のセンサが両方白い場合、直進する { OnFwdSync(OUT_BC,spd,0); } else if ((Sl_l <= black - 5) || (Sl_r <= black - 5)) //どちらかが黒い場合ライントレースをやめる { break; } else //比例制御をする（下で説明） { OnFwd(OUT_B,(spd / (white - black)) * (Sl_l - black) + sign(Sl_l - black) * 10); OnFwd(OUT_C,(spd / (white - black)) * (Sl_r - black) + sign(Sl_r - black) * 10); } } Off(OUT_BC); } 比例制御について説明する。 #ref(2019b/Member/yamaji/Mission3/図.JPG,50%,比例制御) 上の写真では光センサの閾値がwhiteに近づくほど出力が大きくなるような図を書いた。この写真より(spd / (white - black)) * (Sl_l - black)の部分は説明された。しかし、これでは光センサの閾値がblackに近くなると出力が小さくなり過ぎて動かないことがあった。そこで、 sign(Sl_l - black) * 20または10 を加えることで黒に近づいてもある程度の出力が出るように補正した。 **ボールを確保するプログラム [#fd9b4a0e] void catch_ball() //ボールキャッチ { Wait(100); ResetTachoCount(OUT_BC); //角度をリセット OnFwd(OUT_BC,20); //アームとゴム枠を少し後ろに下げる Wait(100); OnFwd(OUT_C,40); //アームを一番後ろまで移動する Wait(500); OnFwd(OUT_B,-20); //ゴム枠を缶に当たるまで下げる Wait(1700); RotateMotor(OUT_B,20,150); //ゴム枠を戻してボールをレールに貯める OnFwd(OUT_C,-35); //下の3行で超音波センサを初期位置にセットする Wait(700); RotateMotor(OUT_B,-20,30); } **ボールを缶に載せるプログラム [#g9f5cf21] #define spd 20 void put_ball() //ボールを置く { RotateMotor(OUT_B,spd,30); //アームを動かすために、少し後ろに移動 OnFwd(OUT_C,2 * spd); //アームを一番後ろまで移動 Wait(1000); RotateMotor(OUT_C,spd,-5); //アームを少し動かし、レールに傾斜をつけボールをゴム枠側へ移動させる Wait(1500); RotateMotor(OUT_B,spd,-120); //空き缶の上にゴム枠とボールをセット Wait(600); RotateMotor(OUT_C,spd,-65); //アームでボールを抑える Wait(400); RotateMotor(OUT_B,spd + 10,50); //アームを下ろしたままゴム枠を上げる Wait(500); OnFwd(OUT_C,2 * spd); //アームを後ろに戻す Wait(1000); RotateMotor(OUT_B,spd,70); //下の3行で超音波センサを初期位置に戻す RotateMotor(OUT_C,spd,-40); RotateMotor(OUT_B,spd,-30); } **その他のプログラム [#sd3e9595] ***超音波センサの50回の平均値を求めるプログラム [#n3133bf0] 超音波センサはたまにおかしな値を出すときがあるため、より確実に超音波センサの値を出すために50回センサの値を測って、その平均をとるようにした。 float Ultrasonic() { long sum = 0; float Sus; SetSensorLowspeed(S4); for (int i = 1;i <= 50;i++) //50回超音波センサの値を足す { sum = sum + SensorUS(S4); } Sus = sum / 50; //超音波センサの値の平均値を求める return Sus; //超音波センサの平均値を返す } ***超音波センサを初期位置に移動するプログラム [#a2f4d532] ロボットを動かし始めたとき、どこに超音波センサがあっても必ず定位置にくるようにアームとゴム枠を使って調整した。アームは少し重いため、出力を2倍にしている箇所がある。 #define spd 20 void initialize() //初期化 { OnFwd(OUT_B,spd); OnFwd(OUT_C,spd*2); //アームとゴム枠を限界まで後ろに移動する Wait(1000); Off(OUT_BC); Wait(500); ResetTachoCount(OUT_BC); //アームとゴム枠の角度をリセット RotateMotor(OUT_C,-spd,40); //アーム移動 Wait(700); RotateMotor(OUT_B,-spd*2,45);//ゴム枠移動 } ***相互通信をしてボールを確保するプログラム（master側） [#q435eb57] メインプログラムをすっきりさせたかったので通信からボール確保までを一連の流れとしてまとめた。 void catchandgo_m() { int msg1_1; //メッセージを受け取ったあとの保存場所として定義 float Sus = Ultrasonic(); //動き始めたときの超音波センサの値を記録 Wait(300); SendRemoteNumber(subs,MAILBOX1,SIGNALON); //スレーブ側のメールボックス1にメッセージを送る while (true) { if (SensorUS(S4) < Sus * (3/5)) //超音波センサの値が初めに測った値の3/5以下になったらスレーブ側のメールボックス2にメッセージを送る { SendRemoteNumber(subs,MAILBOX2,SIGNALON); } ReceiveRemoteNumber(MAILBOX3,true,msg1_1); //スレーブ側からメールボックス3にメッセージを受信したらボールを確保し、スレーブ側のメールボックス4にメッセージを送る if (msg1_1 == SIGNALON) { catch_ball(); Wait(500); SendRemoteNumber(subs,MAILBOX4,SIGNALON); break; } } } ***相互通信をしてライントレースをするプログラム（slave側） [#bef1d009] これもメインプログラムをすっきりさせるために一連の流れとしてまとめたものである。 void catchandgo_s() { int msg2_1,msg2_2; //メッセージを受け取ったあとの保存場所として定義 while(true) { ReceiveRemoteNumber(MAILBOX1,true,msg2_1); //マスター側からメールボックス1にメッセージを受け取ったら音を鳴らしてライントレース if(msg2_1 == SIGNALON) { PlaySound(SOUND_CLICK); follow_line(20000,100000); } ReceiveRemoteNumber(MAILBOX4,true,msg2_2); //マスター側からメールボックス4にメッセージを受け取ったら缶をどかし、音を鳴らしてライントレース if(msg2_2 == SIGNALON) { fwd_back(20); fwd_back(-20); Wait(500); PlaySound(SOUND_LOW_BEEP); follow_line(1000,10000); break; } } } **車体(slave)側のメインプログラム [#u0a41919] task main() { catchandgo_s(); SendResponseNumber(MAILBOX5,SIGNALON); //マスター側のメールボックス5にメッセージを返す catchandgo_s(); Off(OUT_BC); Wait(700); turnAng(-100); //Eで旋回 fwd_back(-5); SendResponseNumber(MAILBOX5,SIGNALON); //マスター側のメールボックス5にメッセージを返す catchandgo_s(); turnAng(-100); //Fで旋回 follow_line(700,10000); turnAng(100); //Gで旋回 follow_line(700,10000); turnAng(100); //Hで旋回 follow_line(700,10000); turnAng(-100); //H'で旋回 follow_line(700,10000); turnAng(100); //G'で旋回 SendResponseNumber(MAILBOX5,SIGNALON); //マスター側のメールボックス5にメッセージを返す **アーム(master)側のメインプログラム [#jf3011ea] task main() { int msg1_2,msg1_3,msg1_4; //メッセージを受け取ったあとの保存場所として定義 SetSensorLowspeed(S4); initialize(); //初期化 Wait(200); catchandgo_m(); catchandgo_m(); //1つ目のボールを取る while(true) { ReceiveRemoteNumber(MAILBOX5,true,msg1_2); //スレーブ側からメールボックス5にメッセージを受信したら、ボールを確保する if(msg1_2 == SIGNALON) { catchandgo_m(); catchandgo_m(); //2つ目のボールを取る break; } } while(true) { ReceiveRemoteNumber(MAILBOX5,true,msg1_3); //スレーブ側からメールボックス5にメッセージを受信したら、ボールを確保する if(msg1_3 == SIGNALON) { catchandgo_m(); catchandgo_m(); //3つ目のボールを取る break; } } while(true) { ReceiveRemoteNumber(MAILBOX5,true,msg1_4); //スレーブ側からメールボックス5にメッセージを受信したら、ボールを缶に乗せる if(msg1_4 == SIGNALON) { put_ball(); put_ball(); //1つ目のボールを置く break; } } } *結果 [#o5d338f5] 私のプログラムは時間が足りずボールを一つ置くまでしかできなかった。そのため、本番ではグループ内の別の人のプログラムを使った。しかし、直前で電池がなくなり、出力が変わってしまったため失敗した。 *反省・感想 [#z2f548b2] まず、今回の課題ではロボットの構造を何度も作り直したり、継ぎ足したり、変えたりと、ハード面で時間を使いすぎてしまった。そのため、プログラムに使う時間が少なくなってしまい十分なプログラムが組めなかった。ロボットの構造は良かったので、前回の反省は生かせたと思う。 このゼミを通して、プログラムの基礎的なことは身についたと思う。また、プログラムは時間のほとんどをデバックに使っていることや、それ故に分かりやすいプログラムを書くことが大切であるということを痛いほど実感した。この経験を今後に生かしていきたいと思う。
