/* 空き缶を探して、空き缶の手前まで進むプログラム */ #define SPEED 50 #define SPEED_SLOW 30 const float diameter = 5.45; // タイヤの直径(cm) const float track = 10.35; // タイヤのトレッド幅(cm) const float pi = 3.1415; // 円周率 void fwdDist(float d) // 距離 d cm 前進 { 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); } int searchDirection(long ang) // 現在の方向を中心に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); // 缶の方向を探した後、近づいて10cm手前で停止 int d = searchDirection(120); if (d > 10) { fwdDist(d-10.0); } }