紙コップに入っている赤ボール2個と青ボール2個を入れ替える
モーター付きの前輪二つと自由に回転するキャスターで車を自由に動かせ、人の手のような関節ロボットを作って、コップを取って、からコップに入れ、もとに戻すロボットである。また、音センサーを利用して、マスターからスレーブ側への命令はbluetoothで、返事は音でした。
コップを待たないで中のボールを取るのは困難であった。そこで、人間のように手を使ってコップを注いで移せばいいと思って左右で回転できるロボットに超音波センサーでコップを探し、カニ手でコップをつかんではこぶことを考えた
prototype
ver 1.0
普通のキャスター付きのロボットはなぜかキャスターが地面にあたり、使えなかった。そこで、4WDのようにホイールを4つつけたら、安定感と90度の正確な回転を得られると思った。左右のモーターにそれぞれギアをつけて2輪ごとに動くようにした。左右を逆回転させばその位置でターンができると考えたが、実際なところホイールの摩擦が多く、思う通りには回らなかった。
ver 2.0
安定性や正確さで問題が現れ改良した。主な問題は画面が見えない、腕が下に傾く、回転が正確ではないである。
のように補強を行うときに画面が見えるように改良した。
また、腕が揺らない、傾かないように反対側から強く固定した。
車輪も回転の時、摩擦を軽減するため、キャスターを付けて改良した。
腕の部分でボールを持ち上げたとき、下に落ちる問題があった。 そこで、蓋を考え、ボールを完全に持ち上げてから垂直に落ちるようにした。
そこで、蓋をするとこのように落ちなくなった。
そこでこのように成功することになった。
上は車の動きである
ボールが入っているコップではコップを掴むというプログラムを送り、蓋を閉め、コップを180度回して、真下まで微調整、蓋を開ける、車若干後進、コップを下す、元の場所に戻るようにした。
また、プログラムでマスターからスレーブ側プログラムを作動させるのは成功したが、返事は失敗し、スレーブ側から音をして、マスター側は反応して動くようにした。
#define SPEED 70 #define SPEED_SLOW 45 #define CONN 1 //スレーブの接続番号 const float diameter = 5.45; //タイヤの直径(cm) const float track = 12; //タイヤのトレッド幅(cm) const float pi = 3.1415; //円周率 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);} } 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センチ後退 Wait(500); ReturnAng(70); //反時計周りに70度旋回 Wait(500); fwdDist(25.0); //30センチ前進 Wait(500); /*ボールの入ったコップの方向を探した後、近づいて10センチ手前で停止*/ SearchCup(8,70); ang1=angle_save; d=d1; until(BluetoothStatus(CONN)==NO_ERR); //接続できるまで待つ /*コップに近づいた後スレーブコップをつかむプログラムを送信*/ RemoteStartProgram(CONN,"hold_sound.rxe"); WAIT();//スレーブが音を発するまで待つ ReturnAng(130); //反時計周り130度旋回 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(120); Wait(500); /*スレーブにコップを離すプログラムを送信*/ 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(); } *スレーブ側 [#ra0ebff7] **コップをつかむプログラム /* モーターAがハサミ、モーターBが腕、モーターCが蓋 */ #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); }
※ OnFwdRegでスピードをOUT_REGMODE_IDLEとしたところ、なぜかプログラム終了時にモーターが少 し逆回転してハサミが勝手に開いてしまったため、この文を加えた。
/* モーターAがハサミ、モーターBが腕、モーターCが蓋 */ #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); }
/* モーターAがハサミ、モーターBが腕、モーターCが蓋 */ #define HOLD OnFwdReg(OUT_A,-40,OUT_REGMODE_IDLE); //コップを挟む #define UP_MAX RotateMotor(OUT_B,-80,185);OnFwdReg(OUT_B,0,OUT_REGMODE_IDLE); //コップを持ち上げてそのままキープ #define CLOSE RotateMotor(OUT_C,50,-120); //蓋を閉める task main() { ResetTachoCount(OUT_ABC); //モーターABCの回転角をリセット /*コップを掴んで蓋をする*/ HOLD; //掴む Wait(1000); CLOSE; //蓋をする Wait(1000); /*180度持ち上げて静止*/ UP_MAX; //もちあげる Wait(500); /*音で作業の終了を伝える*/ PlaySound(SOUND_FAST_UP); Wait(300); PlaySound(SOUND_FAST_UP); Wait(300); Off(OUT_A); //モーターAを切る(※) }
/* モーターAがハサミ、モーターBが腕、モーターCが蓋 */ #define OPEN RotateMotor(OUT_C,50,120); //蓋を開ける task main() { ResetTachoCount(OUT_ABC); //モーターABCの回転角をリセット /*玉を移す*/ OPEN; //蓋を開ける PlaySound(SOUND_FAST_UP); Wait(100); PlaySound(SOUND_FAST_UP); Wait(100); }
/* モーターAがハサミ、モーターBが腕、モーターCが蓋 */ #define DOWN RotateMotor(OUT_B,45,185); //腕を下げる task main() { ResetTachoCount(OUT_ABC); //モーターABCの回転角をリセット /*コップをおろす*/ DOWN; Wait(300); /*音で作業の終了を伝える*/ PlaySound(SOUND_UP); Wait(100); PlaySound(SOUND_UP); Wait(100); }
なぜかコップをつかむプログラムが反対側ではうまくいかなかった。音もしなかったというのはコピーしたコードが動かなかったという事態で解決できなかった。 理由不明で機会があれば、動くか確認がしたい。