robo car 3

測距センサで前方の障害物との距離を測定し自動的に停止する。
障害物が無くなればまた動き始める。

#include "mbed.h"


//-----------------------------------------------------------
Serial pc(USBTX, USBRX);


//-----------------------------------------------------------
// 測距センサクラス
class DistanceSensor
{
public:
    DistanceSensor(PinName in)
    {
        gp2y = new AnalogIn(in);
    };
    
    ~DistanceSensor()
    {
        delete gp2y;
    };

    // ADCの値の読み込み。
    // read_u16()では0Vから3.3Vまでの電圧値を12bit値として読み出す。
    // 12bit値が16bit中、上位12bitに格納されてくる。
    // よって、4bit右シフトして、電圧と値の係数(3300/4095)をかけて
    // 実際の電圧値(ｍV)を取り出す。
    uint16_t value(void)
    {
        uint16_t value;
        value = gp2y->read_u16() >> 4;
        return (value * 3300) / 4095;
    };

    // 電圧値から距離を算出する。
    // GP2Y0A21YK0Fのデータシートから電圧と距離の関係が得られる。
    // その関係は線形ではないので、5cm毎に区切る。5cm区間はそれぞれ
    // 線形と近似する。
    double distance(void)
    {
        //              0cm   5cm  10cm  15cm  20cm  25cm 30cm
        double v[] = { 2800, 2200, 1700, 1300, 1000,  800, 700 };
        double voltage = (double)value();
        int index;
    
        for (index = 0; index<7; index++) {
            if (voltage >= v[index])
                break;
        }
    
        double dis = (index * 5) - ((voltage - v[index]) / (v[index - 1] - v[index]) * 5);
        return dis;
    };

private:
    AnalogIn *gp2y;
};


//-----------------------------------------------------------
// TA7291P モータドライバ制御クラス
class MoterDrive
{
public:
    MoterDrive(PinName in1, PinName in2)
    {
        mIn1 = new PwmOut(in1);
        mIn2 = new PwmOut(in2);

        mIn1->period(0.020);
        mIn2->period(0.020);
    };

    ~MoterDrive()
    {
        delete mIn1;
        delete mIn2;
    };

    /*
     * 引数degreeは-1.0〜1.0の値をとる。
     * 0.0は停止を意味する。
     * 正の値は前進、負の値は後進
     */
    void run(double degree)
    {
        degree = degree < -1.0 ? -1.0 : degree;
        degree = degree >  1.0 ?  1.0 : degree;
                
        if (0.0 < degree){ // go forward
            mIn1->write(degree);
            mIn2->write(0.0);
        }
        else if (degree < 0.0){ // go back
            degree = fabs(degree);
            mIn1->write(0.0);
            mIn2->write(degree);
        }
        else { // degree == 0.0 , then stop
            mIn1->write(0.0);
            mIn2->write(0.0);
        }
    };
        
private:
    PwmOut *mIn1, *mIn2;
};


//-----------------------------------------------------------
// 車両制御クラス
// 前進、後退、回転、停止が可能
class CarControl
{
public:

    CarControl(MoterDrive *l, MoterDrive *r)
    {
        left = l;
        right = r;
    };
    
    ~CarControl(){}
    
    void stop(void)
    {
        left->run(0.0);
        right->run(0.0);
    };

    void forward(double speed)
    {
        left->run(speed);
        right->run(speed);
    };

    void backward(double speed)
    {
        left->run(speed*(-1));
        right->run(speed*(-1));
    };
    
    void rotate(int angle)
    {
        stop();
        if(angle > 0){
            left->run(degree);
            right->run(degree*(-1));
        }
        else if(angle < 0){
            left->run(degree*(-1));
            right->run(degree);
        }
        double waitTime = ((double)abs(angle)) * rotateDeg;
        wait(waitTime);
        stop();
    };

private:
    MoterDrive *left, *right;
    
    static const double degree = 0.9;
    static const double rotateDeg = 0.01;
};


//-----------------------------------------------------------
class Led
{
public:
    Led()
    {
        led[0] = new DigitalOut(LED1);
        led[1] = new DigitalOut(LED2);
        led[2] = new DigitalOut(LED3);
        led[3] = new DigitalOut(LED4);
    }
    
    ~Led()
    {
        for(int i=0;i<4;i++){
            delete led[i];
        }
    }

    // レベルに応じてLEDを点灯する。
    // level : 0 - 4
    void indicator(int level)
    {
        level = level < 0 ? 0 : level;
        level = level > 4 ? 4 : level;
        
        switch(level){
        case 0: *led[0] = 0; *led[1] = 0; *led[2] = 0; *led[3] = 0; break;
        case 1: *led[0] = 1; *led[1] = 0; *led[2] = 0; *led[3] = 0; break;
        case 2: *led[0] = 1; *led[1] = 1; *led[2] = 0; *led[3] = 0; break;
        case 3: *led[0] = 1; *led[1] = 1; *led[2] = 1; *led[3] = 0; break;
        case 4: *led[0] = 1; *led[1] = 1; *led[2] = 1; *led[3] = 1; break;
        }
    }
    
    // LEDフラッシャー
    // LPC1768ボード上の4つのLEDを順に付けて、消す。
    void flasher() {
        for(int i=0;i<4;i++){
            *led[i] = 1;
            wait(0.25);
        }
        for(int i=0;i<4;i++){
            *led[i] = 0;
            wait(0.25);
        }
    };

    // On/Offトグル
    void toggle(int index)
    {
        int s = *led[index];
        *led[index] = s == 0 ? 1 : 0;
    }

    void on(int index) { *led[index] = 1; }    // 点灯
    void off(int index){ *led[index] = 0; }    // 消灯
    
private:
    DigitalOut *led[4];
};



//-----------------------------------------------------------
// DRIVE-2
// 障害物に近づくまで前進。ランダムに回転(-180°〜180°)
// また障害物を見つけるまで前進、を繰り返す。
void drive2(void)
{
    MoterDrive *md1 = new MoterDrive(P2_5, P2_4);
    MoterDrive *md2 = new MoterDrive(P2_3, P2_2);
    CarControl *cc = new CarControl(md1, md2);
    DistanceSensor *sensor = new DistanceSensor(P1_31);
    Led leds;
    
    leds.flasher();

    cc->forward(0.7);

    while(1){
        wait(0.1);

        double d = sensor->distance();
        leds.toggle(0);
        
        if (d < 5){
            leds.on(3);
            cc->backward(0.6);
            wait(1.0);
            cc->stop();
            wait(1.0);
            leds.off(3);
            
            leds.on(2);
            int r = (rand() % 360) - 180;
            cc->rotate(r);
            leds.off(2);
        }
        else if (d < 10){
            cc->forward(0.2);
        }
        else if (d < 15){
            cc->forward(0.4);
        }
        else {
            cc->forward(0.7);
        }
    }

}


//-----------------------------------------------------------
int main() {
    pc.printf("--- ROBO CAR 3 ---\n");
    drive2();
    while(1){ wait(1.0); }
}