2018a/Mission3
空き缶を探して、空き缶の手前まで進むプログラム例†
/*
空き缶を探して、空き缶の手前まで進むプログラム
*/
#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, 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){
d = SensorUS(S1); // 現在の距離
if (d < d_min){
d_min = d; // 仮の最小値を更新
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);
}
}