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);
    }
}

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 単語検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2018-07-20 (金) 17:24:33 (148d)