何度も作り変え、最終的にはこのような形になりました。この1台で行います。
このロボットの動きはまず、本体正面に付けた超音波センサでコップを探し、カニの手のようなアームでコップを掴みます。
掴んだ後にコップをひっくり返して移動をするので、ボールが落ちないように蓋をつけました。蓋もモーターで動きます。
コップを掴んだあとは、再びセンサで空のコップをさ探して近づき、カニの手でコップを持ち上げ傾けてボールを注ぐ、という動きでボールを移し替えます。
片面でこの動きができれば、もう片方でも同じプログラムを動かせば良いだろうと考えました。
また、改良前の段階で、カニの手のアームを固定させるためにモニターの上にたくさん部品を載せてしまい、モニターが見えなくなるという事態が生じました。
それだと作業効率が悪いので、このようにしてしっかり見えるように改良しました。
また、少ない部品でしっかり固定をするように工夫をしました。これなら持ち上げても外れません。
また、サンプルのロボットのキャスターを真似してキャスターを取り付けました。これにより、旋回の動きがスムーズになりました。
右側に腕があるので、後ろ向きでスタートします。まず、1の場所まで後退します。(1)
1の場所で半時計回りに旋回し少し前進して2の場所まで行き(2)、60度の範囲でコップを探します。(3)
コップの位置と距離を確認して、10センチ手前まで進み、反時計周りに旋回します。(4)
スレーブに「コップを掴む」というプログラムを起動させます。(5)
スレーブは、コップを掴んだあとに音を発します。この音を感知するまで待ちます。(6)
コップを掴んだら、その場で反時計回りに旋回し、次は空のコップのある4の場所を目指します。(7)
少し後退することで、正確にコップの位置を把握するために、ある程度の距離をとります。(8)
そのあとは、最初と同様に、超音波センサでコップの位置と距離を測り、10センチ手前まで進みます。(9)
スレーブにそそぐプログラムを送信し(10)、コップを傾ける前にふたをさせます。
コップを持ち上げた状態で反時計回りに旋回し、ふたを開けます。(11)
持ち上げた状態で再び旋回し(12)、腕をおろすプログラムを送信します。(13)
ボールを移したので、次は元の位置にコップを戻す動作にいきます。
カニの手でコップを倒さないように、少し後退し、反時計回りに旋回します。(14)
この動作は、何度も試して、具体的な値を入れて動かしています。
この動きでだいたい3の場所あたりに移動できるので、コップを離すプログラムを送信します。(15)
そのあとは、反対側のフィールドに移動をして、(3)あたりからの動作をさせるプログラムになっています。※反対側に移動してからの動作は、うまくいきませんでした。
#define SPEED 70 #define SPEED_SLOW 45 #define CONN 1 //スレーブの接続番号
2つのNXTをBluetoothで接続します。向かって右側がマスター、腕の付いている左側がスレーブです。
スレーブ側には腕の、マスター側にはコップを探して近づくプログラムを転送しておきます。 すべての指示をマスターが送ります。
以下のように、先生からもらったサンプルのプログラムを参考に作りました。
const float diameter = 5.45; //タイヤの直径(cm) const float track = 12; //タイヤのトレッド幅(cm) const float pi = 3.1415; //円周率 long angle_save; void fwdDist(float d) //距離dcm前進 { long angle; angle = d/(diameter*pi)*360.0; //角度を計算する RotateMotorEx(OUT_BC, SPEED_SLOW, angle, 0, true, true); } void turnAng(long ang) //角度ang度の時計回りの旋回 { long angle; angle = track/diameter * ang; RotateMotorEx(OUT_BC, SPEED_SLOW, angle, 100, true, true); } void ReturnAng(long ang) //角度ang度の半時計回りの旋回 { Wait(500); long angle; angle = track/diameter * ang; RotateMotorEx(OUT_BC, SPEED_SLOW, angle, -100, true, true); } int searchDirection(long ang) //現在の方向を中心にang度の範囲で探し、障害物までの距離を返す { long angle,tacho_min=0, tacho_corr; int d_min; d_min=600; //仮の最小値 angle = (track/diameter)*ang; //旋回角度からタイヤの回転を計算 turnAng(ang/2); //指定された角度の半分を旋回 ResetTachoCount(OUT_BC); //角度計測をリセット OnFwdSync(OUT_BC,SPEED_SLOW-15,-100); //半時計回りに旋回 while(MotorTachoCount(OUT_B)<=angle){ if(SensorUS(S1)<d_min){ d_min=SensorUS(S1); tacho_min=MotorTachoCount(OUT_B); } } angle_save=tacho_min; OnFwdSyncEx(OUT_BC,SPEED_SLOW,100,RESET_NONE); until(MotorTachoCount(OUT_B)<=tacho_min||SensorUS(S1)<=d_min); Wait(14); //微調整 Off(OUT_BC);Wait(500); return d_min; }
これは本体の上に付けた音のセンサで使用します。スレーブが動作を終えたあとに鳴らす音を感知して、その動作が終わったことを確認してから次の動作に移るためです。
void WAIT(){ while(Sensor(S2)>99)//返事(音)を待つ {Wait(1);} }
スタートする時からカニの爪を開いたままコップの手前まで行きます。そのあと半時計回りに旋回してコップをキャッチします。この一連の動作を以下のように「SearchCup」という命令にしました。
void SearchCup(int a, int b)//a=何cm手前で停止、b=行って何度回転する {int d = searchDirection(60); if (d > a) { fwdDist(d-a); } Wait(100); ReturnAng(b); //反時計周りにb度旋回 Off(OUT_BC); }
これは、逆の動作をさせるためのプログラムです。コップを持って行く過程を逆にたどれば、元々コップの置いてあった場所まで戻れると考えましたが、思っていたようには動かなかったので、使用せず他の方法を取りました。
void comeback(int d, int cm, int turn, int angle){ ReturnAng(angle); fwdDist(-d); turnAng(turn); }
task main() { SetSensorLowspeed(S1); //端子1に超音波センサ SetSensorSound(S2); //端子2に音のセンサ int ang1,ang2,d1,d2,d; fwdDist(-40.0); //50センチ後退(1) Wait(500); ReturnAng(70); //反時計周りに70度旋回(2) Wait(500); fwdDist(25.0); //25センチ前進 (3) Wait(500); /*ボールの入ったコップの方向を探した後、近づいて10センチ手前で停止*/(4) SearchCup(8,70); ang1=angle_save; d=d1; until(BluetoothStatus(CONN)==NO_ERR); //接続できるまで待つ /*コップに近づいた後スレーブコップをつかむプログラムを送信*/(5) RemoteStartProgram(CONN,"hold_sound.rxe"); WAIT();//スレーブが音を発するまで待つ(6) ReturnAng(130); //反時計周り130度旋回(7) Wait(500); fwdDist(-15); //15cm後退(8) /*空のコップの方向を探した後、近づいて10センチ手前で停止*/(9) SearchCup(11,0); ang2=angle_save; d=d2; /*空のコップに近づいた後スレーブにボールを注ぐプログラムを送信*/(10) RemoteStartProgram(CONN,"pour_up.rxe"); Wait(5000); ReturnAng(50); //反時計回りに50度旋回 RemoteStartProgram(CONN,"pour_open.rxe");(11) Wait(3000); turnAng(-40);(12) RemoteStartProgram(CONN,"pour_down.rxe");(13) Wait(3000); fwdDist(-20); Wait(500); ReturnAng(120);(14) Wait(500); /*スレーブにコップを離すプログラムを送信*/(15) RemoteStartProgram(CONN,"release_sound.rxe"); WAIT(); turnAng(90); fwdDist(-25); Wait(500); turnAng(-70); Wait(500); fwdDist(30); SearchCup(8,70); /*ボールの入ったコップの方向を探した後、近づいて10センチ手前で停止*/ SearchCup(8,70); ang1=angle_save; d=d1; until(BluetoothStatus(CONN)==NO_ERR); //接続できるまで待つ /*コップに近づいた後スレーブコップをつかむプログラムを送信*/ RemoteStartProgram(CONN,"hold_sound.rxe"); WAIT();//スレーブが音を発するまで待つ ReturnAng(130); //反時計周りに360度旋回 Wait(500); fwdDist(-15); //10cm後退 /*空のコップの方向を探した後、近づいて10センチ手前で停止*/ SearchCup(11,0); ang2=angle_save; d=d2; /*空のコップに近づいた後スレーブにボールを注ぐプログラムを送信*/ RemoteStartProgram(CONN,"pour_up.rxe"); Wait(5000); ReturnAng(50); RemoteStartProgram(CONN,"pour_open.rxe"); Wait(3000); turnAng(-40); RemoteStartProgram(CONN,"pour_down.rxe"); Wait(3000); fwdDist(-20); Wait(500); ReturnAng(90); Wait(500); /*スレーブにコップを離すプログラムを送信*/ RemoteStartProgram(CONN,"release_sound.rxe"); WAIT(); }
コップを掴むプログラム
/* モーターAがハサミ、モーターBが腕 */ #define HOLD OnFwdReg(OUT_A,-40,OUT_REGMODE_IDLE); //コップを挟む task main() { HOLD; //掴む Wait(500); //ちょっと待つ Off(OUT_A); //モーターAを切る Wait(500); PlaySound(SOUND_UP); Wait(100); PlaySound(SOUND_UP); Wait(100); }
コップを離すプログラム
/* モーターAがハサミ、モーターBが腕 */ #define RELEASE RotateMotor(OUT_A,50,50); //コップをはなす task main() { ResetTachoCount(OUT_AB); //モーターABの回転角をリセット RELEASE; //コップを放す Wait(300); PlaySound(SOUND_DOWN); Wait(100); PlaySound(SOUND_DOWN); Wait(100); }