自走式光追尾カー

CdSフォトトランジスタをサーボモータの上につけ、光の最も強い方向を探す。
見つけた方向に車体を回転させ、少し前進。
を繰り返し、光源に近づいていく。

#include "mbed.h"

Serial pc(USBTX, USBRX);

//-----------------------------------------------------------
// CLASS for DC moter driver 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;
    };

    // parameter degree takes the value -1.0 ^ 1.0
    // 0.0 means stop
    // positive value means go forward
    // negative value means fo backward
    void run(double degree)
    {
        if (degree < -1.0)
        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 for servo moter SG90
class ServoMoter
{
public:
    ServoMoter(PinName in)
    {
        servo = new PwmOut(in);

        servo->period(0.020);
    };

    ~ServoMoter()
    {
        delete servo;
    };
    
    // parameter degree takes -90 ~ 90
    // it represents axis
    // About SG90
    // duty range is 0.5ms ~ 2.4ms corresponds to -90 degree to 90 degree.
    // especially, 1.45ms means 0 degree.
    // the relation degree and duty cycle is below.
    // y = (1.9/180)*x + 1.45
    void run(int degree)
    {
        degree = degree < -90 ? -90 : degree;
        degree = degree >  90 ?  90 : degree;
        double duty = (1.9/180)*(double)degree + 1.45;
        servo->pulsewidth(duty/1000.0);
    };
        
private:
    PwmOut *servo;

};



//-----------------------------------------------------------
// CLASS for Photo register CDS MI5 series
class PhotoRegister
{
public:
    PhotoRegister(PinName in)
    {
        photo = new AnalogIn(in);
    };
    
    ~PhotoRegister()
    {
        delete photo;
    };

    uint16_t value(void)
    {
        return photo->read_u16();
    };

private:
    AnalogIn *photo;
};


//-----------------------------------------------------------
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(void)
    {
        left->run(degree);
        right->run(degree);
    };

    void backward(void)
    {
        left->run(degree*(-1));
        right->run(degree*(-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;
        pc.printf("rotate time : %f\n", waitTime);
        wait(waitTime);
        stop();
    };

private:
    MoterDrive *left, *right;
    
    static const double degree = 0.5;
    static const double rotateDeg = 0.02;
};


//-----------------------------------------------------------
class SearchLihgt
{
public:
    SearchLihgt(PhotoRegister *sensor, ServoMoter *servo)
    {
        pr = sensor;
        sm = servo;
        
        measures[0].angle = 90;
        measures[1].angle = -90;
        measures[2].angle = 75;
        measures[3].angle = -75;
        measures[4].angle = 60;
        measures[5].angle = -60;
        measures[6].angle = 45;
        measures[7].angle = -45;
        measures[8].angle = 30;
        measures[9].angle = -30;
        measures[10].angle = 15;
        measures[11].angle = -15;
        measures[12].angle = 0;
    };

    ~SearchLihgt(){}
    
    int search(void)
    {
        int index;
        int maxCandidate = 0;
        uint16_t candidateValue = 0;
        for(index = 0;index<13;index++){
            sm->run(measures[index].angle);
            wait(0.5);
            measures[index].value = pr->value();
            
            if(measures[index].value > candidateValue){
                maxCandidate = index;
                candidateValue = measures[index].value;
            }
//            pc.printf("search() %d : %d : %d\n", index, measures[index].angle, measures[index].value);

        }
//        pc.printf("Found min pos : %d\n", maxCandidate);
        
        sm->run(measures[maxCandidate].angle);
        return measures[maxCandidate].angle;
    };

    struct pair
    {
        int angle;
        uint16_t value;
    };
    
    struct pair measures[13];
        
private:
    PhotoRegister *pr;
    ServoMoter *sm;
    
};



//-----------------------------------------------------------
class Led
{
public:
    Led(PinName pin)
    {
        led = new DigitalOut(pin);
        *led = 0;
        state = 0;
    };

    void toggle(void)
    {
        if (state == 0){
            *led = 1;
            state = 1;
        }
        else{
            *led = 0;
            state = 0;
        }
    };

private:
    DigitalOut *led;
    int state;
};


//-----------------------------------------------------------
void autoDrive(void)
{
    MoterDrive *md1 = new MoterDrive(P2_1, P2_0);
    MoterDrive *md2 = new MoterDrive(P2_3, P2_2);
    ServoMoter *sm  = new ServoMoter(P2_5);
    PhotoRegister *pr = new PhotoRegister(P1_31);

    CarControl *cc = new CarControl(md1, md2);
    SearchLihgt *sl = new SearchLihgt(pr, sm);
    
    Led *led = new Led(LED1);
    
    for(int count=0;;count++){

        cc->stop();
        int angle = sl->search();
        wait(1.0);
        led->toggle();
        pc.printf("DC_moter %d : %d\n", count / 100, angle);
        cc->rotate(angle);
        wait(1.0);
        led->toggle();
        cc->forward();
        wait(2.0);
        led->toggle();
    }
}


//-----------------------------------------------------------
int main() {
    autoDrive();

    while(1){}
}