今回製作したロボットの特徴はマシン前方に超音波センサがついていることです。 このマシンは箱を反対側のコートに送るときに箱を回転ますが、マシンギリギリで箱をつかんでしまうと回転させることができません。 そこでこのセンサを使用することで掴みたい位置で箱を掴むことができます。 また、この機能は送られてきた箱も検出できるので動作の誤差を減らすことにもつながります。
このロボットは箱を掴むとき、2本のアームではさんでいます。箱に接している部分は細い車輪なので狭いです。箱を反対側のコートに送るときに箱を回転させる軸になる部分です。
箱を反対側のコートに送れたことをタッチセンサで確認しています。 タッチセンサが反応したらアームが開き箱を置いていきます。
if文を使うと超音波センサで箱を検出する際にプログラムの形が分かりにくくなったため、今回もswitch-case文を使うことにしました。 この形の文を書くとプログラムを部品ごとに分けることが出きるのでプログラムの動作確認や追加がしやすいです。実際にcase5は後から追加されたため順番が動作の流れ通りになっていません。
#define SPD 40 //直進するスピード #define SPD_H 30 //旋回するスピード #define SPD_Ah1 -20 //アームの閉じる速度(箱1つの場合) #define SPD_Ah2 -50 //アームの閉じる速度(箱2つの場合) #define SPD_Ar 30 //アームの開く速度 #define TIM_W 800 //動作準備のための待機時間 /* 方向修正するプログラムのパーツ */ #define trasew1 OnFwdReg(OUT_BC,SPD,OUT_REGMODE_SYNC);Wait(1); // 直進する #define trasew2 OnFwd(OUT_C,SPD);Off(OUT_B);Wait(1); // 方向修正する task main () { SetSensorLight(S1); //ライトセンサー定義 SetSensorLowspeed(S2); //超音波センサー定義 SetSensorTouch(S3); //タッチセンサー定義 int pattern = 1; //パターン設置 int t1 = 0; //方向修正のカウンタ int count = 0; // 箱が1つの場合と2つの場合を分けるフラグ while(true){ switch(pattern){ case 1: //箱に至るまでの直進 if(SensorUS(S2) > 8){ if(t1 <= 300){ // 直進の方向修正をするプログラム t1++; OnFwdReg(OUT_BC,SPD,OUT_REGMODE_SYNC); Wait(1); }else{ t1 = 0; trasew2; } break; } pattern = 2; break; case 2: //箱をつかむ if(count == 1){ OnFwdReg(OUT_BC,SPD,OUT_REGMODE_SYNC); Wait(550); } Off(OUT_BC); Wait(TIM_W); if(count == 0){ OnFwd(OUT_A,-SPD_Ah1); Wait(TIM_W); pattern = 5; break; } OnFwd(OUT_A,-50); Wait(TIM_W); pattern = 5; break; case 5: //壁に向く if(count == 0){ OnFwdReg(OUT_BC,-SPD,OUT_REGMODE_SYNC); Wait(3600); OnFwd(OUT_B,SPD_H); OnFwd(OUT_C,-SPD_H); Wait(1600); Off(OUT_BC); Wait(TIM_W); pattern = 3; break; } OnFwdReg(OUT_BC,-SPD,OUT_REGMODE_SYNC); Wait(4200); OnFwd(OUT_B,SPD_H); OnFwd(OUT_C,-SPD_H); Wait(1560); Off(OUT_BC); Wait(TIM_W); pattern = 3; break; case 3: //タッチセンサーが接触するまで前進してそこからまた箱に向き直す if(SENSOR_3 == 0){ if(t1 <= 1000){ // 直進の方向修正をするプログラム t1++; OnFwdReg(OUT_BC,SPD,OUT_REGMODE_SYNC); Wait(1); }else{ t1 = 0; trasew2; } break; } Off(OUT_BC); OnFwd(OUT_A,SPD_Ar); Wait(TIM_W); Off(OUT_A); if(count == 0){ OnFwdReg(OUT_BC,-SPD,OUT_REGMODE_SYNC); Wait(3000); OnFwd(OUT_B,-SPD_H); OnFwd(OUT_C,SPD_H); Wait(1200); count++; Off(OUT_BC); pattern = 1; break; } OnFwdReg(OUT_BC,-SPD,OUT_REGMODE_SYNC); Wait(1550); OnFwd(OUT_B,-SPD_H); OnFwd(OUT_C,SPD_H); Wait(1100); Off(OUT_BC); pattern = 4; break; case 4://箱を枠の中に押し込む if(SensorUS(S2) > 8){ if(t1 <= 90){ // 直進の方向修正をするプログラム t1++; OnFwdReg(OUT_BC,SPD,OUT_REGMODE_SYNC); Wait(1); }else{ t1 = 0; trasew2; } break; } Wait(TIM_W); OnFwdReg(OUT_BC,SPD,OUT_REGMODE_SYNC); Wait(TIM_W); Off(OUT_BC); OnFwd(OUT_A,-30); Wait(TIM_W); OnFwd(OUT_B,-SPD_H); OnFwd(OUT_C,SPD_H); Wait(800); OnFwdReg(OUT_BC,SPD,OUT_REGMODE_SYNC); Wait(880); Off(OUT_BC); Wait(30000); break; } } }