今回の課題は空き缶を探したあとにそれに向かってシュートするというもの。
新たに付け足した部分は、アームの上の超音波センサー。 少し不格好ではあるが、正面を向いているのでプログラムを組みやすかった。今回も課題2に記したようにコンピュータは固定せず、車体にのせているだけではあるが、特に問題はなかった。
サンプルプログラムを参考にして、機体の角度を使って空き缶の位置を割り出した。
#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) {
角度を計算し、angle度まで回転させる。
long angle; angle = d/(diameter*pi)*360.0; RotateMotorEx(OUT_BC, SPEED_SLOW, angle, 0, true, true); }
ang度時計回りに旋回
void turnAng(long ang) { long angle; angle = track/diameter * ang; RotateMotorEx(OUT_BC, SPEED_SLOW, angle, 100, true, true); }
ang度の範囲探す。
int searchDirection(long ang) {
旋回角度からタイヤの回転を計算し、そこまで反時計回りで戻る。
long angle,tacho_min=0, tacho_corr ; int d_min; d_min=300; angle = (track/diameter)*ang; turnAng(ang/2); ResetTachoCount(OUT_BC); OnFwdSync(OUT_BC,SPEED_SLOW,-100); while(MotorTachoCount(OUT_B)<=angle){ if(SensorUS(S1)<d_min){ d_min=SensorUS(S1); tacho_min=MotorTachoCount(OUT_B); } } 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; }
缶へシュートする。
task main() { SetSensorLowspeed(S1); int d = searchDirection(120); if (d > 10) { fwdDist(d-25.0); OnRev(OUT_A,30); Wait(400); Off(OUT_A); OnFwd(OUT_BC,50); Wait(400); } }
角度を扱う方式でプログラムを作った。なかなか複雑で難しかったがいい経験になった。 これをうまく使えば一気にプログラムの自由度が増すなと思った。