課題:ボールを離れた場所から缶に当てるロボットとボールを回収するロボット
以下のルールに沿ってタイムレースを行う。
私達はボールを放つロボットAを作成した
課題2で使用したライントレースのロボットにいくつかパーツを追加した
モーターA,Cで車輪を、モーターBでボールを掴み射出するためのアームを操作する
ボールを掴むとこのようになる
ボールを打ち出すときはアームを逆回転させることでボールを蹴り出すような要領で行う。
ライントレースのための光センサーとボールや缶を認識するための超音波センサーは前部に取り付けた
起動するとまずP点のボールを探して掴みに行く。
次に缶の前まで移動、缶の方を向いてロボットBへ信号を発して返信があるまで待つ。
返信が来たところでボールを発射、Q点のボールを掴みに行く。
ボールを掴んで少し移動したところでプログラム終了。
#define SPEED 50 #define SPEED_SLOW 30 const float diameter = 5.45; const float track = 10.35; const float pi = 3.1415; void fwdDist (float d){ // dセンチ前進させる処理 long angle; angle = d / (diameter * pi) * 360.0; RotateMotorEx(OUT_AC,SPEED_SLOW,angle,0,true,true); } void turnAng(long ang){ // ang度旋回させる処理 long angle ; angle = track/diameter*ang; RotateMotorEx(OUT_AC,SPEED_SLOW,angle,100,true,true); } int searchDirection(long ang){ // 近くにある障害物を探してその方向を向き、ボールまでの距離を返す long angle, tacho_min; int d_min; d_min = 300; tacho_min = 0; angle = (track / diameter) * ang; turnAng(ang / 2); ResetTachoCount(OUT_A); OnFwdSync(OUT_AC,SPEED_SLOW,-100); while(MotorTachoCount(OUT_A) <= angle){ if(SensorUS(S3) < d_min){ d_min = SensorUS(S3); tacho_min = MotorTachoCount(OUT_A); } } OnFwdSyncEx(OUT_AC,SPEED_SLOW,100,RESET_NONE); until(MotorTachoCount(OUT_A) <= tacho_min || SensorUS(S3) <= d_min); Wait(15); Off(OUT_AC); Wait(500); return d_min; } void searchDirection2(long ang){ // 近くの障害物を探してその方向を向く処理。距離は返さない long angle, tacho_min; // 何故か searchDirection を2回利用できなかったため急遽作成 int d_min; d_min = 300; tacho_min = 0; angle = (track / diameter) * ang; turnAng(ang/2); ResetTachoCount(OUT_C); OnFwdSync(OUT_AC,SPEED_SLOW,-100); while(MotorTachoCount(OUT_C) >= angle*-1){ if(SensorUS(S3) < d_min){ d_min = SensorUS(S3); tacho_min = MotorTachoCount(OUT_C); } } OnFwdSyncEx(OUT_AC,SPEED_SLOW,100,RESET_NONE); until(MotorTachoCount(OUT_C) >= tacho_min || SensorUS(S3) <= d_min); Wait(15); Off(OUT_AC); Wait(500); } void line () { // ライントレースの処理 float d; int S = 0; SetSensorLight(S2); long t_L = CurrentTick(); while(S<3){ while(CurrentTick() - t_L <= 170) { if (SENSOR_2 < 30) { OnFwd(OUT_C,30); OnRev(OUT_A,25); } else if (SENSOR_2 < 38) { OnFwd(OUT_C,30); Off(OUT_A); } else if (SENSOR_2 < 52) { OnFwd(OUT_AC,30); t_L = CurrentTick(); } else if (SENSOR_2 < 60) { OnFwd(OUT_A,30); Off(OUT_C); t_L = CurrentTick(); } else { OnFwd(OUT_A,30); OnRev(OUT_C,25); t_L = CurrentTick(); } Wait(1); } S++; if(S==1){ // 交差点1回目(点P)を左折してからQ点のボールを狙う Off(OUT_AC); Wait(1000); RotateMotor(OUT_A,SPEED,210); OnFwdSync(OUT_AC,SPEED_SLOW,0); Wait(1000); d = searchDirection(120); fwdDist(d - 8.0); RotateMotor(OUT_C,SPEED_SLOW,150); // 拾う前微調整 RotateMotor(OUT_B,33,80); // 1回目にボールを放ったためにアームが開きすぎているため少し閉じる OnFwdSync(OUT_AC,SPEED,0); // ボール拾い Wait(450); RotateMotor(OUT_B,33,80); Off(OUT_AC); t_L = CurrentTick(); } else if(S==2){ // 交差点2回目(どこかわからない)ではライントレースの処理を終了 break; } } } task main(){ SetSensorLowspeed(S3); SetSensorLight(S2); int mail = 0; int d = searchDirection(120); // ボール探し fwdDist(d - 8.0); RotateMotor(OUT_C,SPEED_SLOW,125); // 拾う前微調整 OnFwdSync(OUT_AC,SPEED,0); // ボール拾い Wait(450); RotateMotor(OUT_B,33,80); Off(OUT_AC); fwdDist(-5); // つかんだ後5cm後ろへ進む RotateMotor(OUT_C,SPEED_SLOW,-300); // 缶付近に移動 OnFwdSync(OUT_AC,SPEED,0); Wait(1800); searchDirection2(90); // 角度合わせ RotateMotor(OUT_C,SPEED_SLOW,90); // 発射前微調整 SendRemoteNumber(1,MAILBOX1,10); // キャッチ側を移動させる為に発信 while(mail != 20){ // キャッチ準備完了待ち Off(OUT_ABC); Wait(1); ReceiveRemoteNumber(MAILBOX1,false,mail); } OnFwd(OUT_B,-70); // ボール発射 Wait(800); Off(OUT_B); turnAng(210); // 後ろを向いてライントレース準備のための移動 OnFwd(OUT_AC,1); Wait(1); OnFwd(OUT_A,SPEED_SLOW); OnFwd(OUT_C,SPEED_SLOW - 3); until(SENSOR_2 < 40); Off(OUT_A); line(); // ライントレース RotateMotor(OUT_A,SPEED_SLOW,220); // 処理を終了する前に適当に移動させる OnFwd(OUT_AC,1); Wait(1); fwdDist(15.0); Off(OUT_ABC); }
調整の時間があまりとれなかったという事もあるが、有効な解決策を見つけられないまま問題の放置をしてしまったことが一番の反省点であると思う。
特に超音波センサーによるボールの検知に関して、スタート地点からすぐにボールを探すのではなく、今回の課題の場合にはボールの位置が固定されているから、ある程度ボールに近づいてからボールの探索を行うようにするという方法もあっただろう。
2つ目のボールに関してはsearchDirectionを複数回使用できない問題もあり、「とりあえず追加した」というような形になってしまった。これに関しても事前の調整の時間をきちんととれていればP点のボールや缶を発見するときとは違った方法で発見して拾うことが出来たのではないかと思う。