Takuya Kobayashi / Mbed 2 deprecated mbed_linetrace_termB_test

Dependencies:   mbed

main.cpp

Committer:
kamorei
Date:
2018-10-26
Revision:
0:8b8f5dba70e9
Child:
1:905436937f78

File content as of revision 0:8b8f5dba70e9:

//直線だけP制御したら直線でブレなくなるかもしれないので作りました

#include "mbed.h"

DigitalOut ledL( PTB8);
DigitalOut ledR( PTE5);
BusOut ledLL( PTB8, PTB9);
BusOut ledRR( PTE5, PTE4);  //わざと右から書いてます

AnalogIn sensorR( PTB1);
AnalogIn sensorL( PTB3);
AnalogIn sensorCR( PTB0);
AnalogIn sensorCL( PTB2);

//モータ1
BusOut Mlefti(PTA1, PTA2);
PwmOut Mleftp(PTD4);
//モータ2
BusOut Mrighti(PTC0, PTC7);
PwmOut Mrightp(PTA12);

float white = 0.6, black = 0.08, gray = 0.3;   //値をぶち込む
float whiteR = 0.02, blackR = 0.008, grayR = 0.015; //弱いセンサ用
float sensor[4];    //sensor[0]:sensorL ... sensor[3]:sensorR
float kp = 0.5; //Pゲイン
float pr, pl;
int i = 0, j = 0;

void turn_right(){
    Mlefti = 2;
    Mleftp = 0.7f;
    Mrighti = 1;
    Mrightp = 0.7f;
    ledR = 1;
    ledL = 0;
}

void turn_left(){ 
    Mrighti = 2;
    Mrightp = 0.7f;
    Mlefti = 1;
    Mleftp = 0.7f;    
    ledR = 0;
    ledL = 1;
}

void go_straight( float r, float l){
    Mrighti = 2;
    Mrightp = (kp * pr + 0.5) * 1.0f;
    Mlefti = 2;
    Mleftp = (kp * pl + 0.5) * 1.0f;
    ledR = 0;
    ledL = 0;
}

void go_back( float r, float l){
    Mrighti = 1;
    Mrightp = (kp * pr + 0.5) * 1.0f;
    Mlefti = 1;
    Mleftp = (kp * pl + 0.5) * 1.0f;
    ledR = 0;
    ledL = 0;
} 

void stop_point(){
    Mrighti = 0;
    Mlefti = 0;
    ledRR = 2;
    ledLL = 2;
}

int main() {
    go_straight();
    wait(0.2); 
    while( i < 3){   
        while(1) {
            sensor[0] = sensorL.read();
            sensor[1] = sensorCL.read();
            sensor[2] = sensorCR.read();
            sensor[3] = sensorR.read();
        
            pr = (sensor[2] - black) / (white - black);
            pl = (sensor[1] - black) / (white - black);
        
            if( sensor[2] <= gray)
                turn_right();
            else if( sensor[1] <= gray)
                turn_left();
            else if( sensor[1] <= gray && sensor[2] <= gray){
                if(ledR == 1)
                    turn_right;
                else
                    turn_left;
            }
            else
                go_straight( pr, pl);
            if( sensor[0] <= black && sensor[3] <= blackR){
                stop_point();
                break;
            }
        }
        i++;
        if( i < 3){
/*            while(1){
                sensor[0] = sensorL.read();
                sensor[1] = sensorCL.read();
                sensor[2] = sensorCR.read();
                sensor[3] = sensorR.read();
                
                pr = (sensor[2] - black) / (white - black);
                pl = (sensor[1] - black) / (white - black);
                
                if( sensor[0] <= black && sensor[3] <= blackR){
                    stop_point();
                    break;
                }
                else
                    go_back( pr, pl);
            }
*/
            for( j = 0; j < 2;){
                turn_left();
                if( sensor[2] <= black)
                    j++;
            }
        }
    }
}