Takuya Kobayashi / Mbed 2 deprecated mbed_linetrace_termA

Dependencies:   mbed

main.cpp

Committer:
kamorei
Date:
2018-10-22
Revision:
2:bbd10b5da0bf
Parent:
1:5a97a5e4ee44
Child:
3:bc6c111b88da

File content as of revision 2:bbd10b5da0bf:

#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.5, black = 0.08, gray = 0.15;   //値をぶち込む
float whiteR = 0.02, blackR = 0.008, grayR = 0.015; //弱いセンサ用
float sensor[4];    //sensor[0]:sensorL ... sensor[3]:sensorR

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

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

void go_straight(){
    //まっすぐ行こう
    Mrighti = 2;
    Mrightp = 0.8f;
    Mlefti = 2;
    Mleftp = 0.8f;
    ledR = 0;
    ledL = 0;
}

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

int main() {
    go_straight();
    wait(0.2);    
    while(1) {
        sensor[0] = sensorL.read();
        sensor[1] = sensorCL.read();
        sensor[2] = sensorCR.read();
        sensor[3] = sensorR.read();
        
        if( sensor[1] >= white && sensor[2] <= gray)
            turn_right();
        else if( sensor[1] <= gray && sensor[2] >= white)
            turn_left();
        else
            go_straight();
        if( sensor[0] <= gray && sensor[3] <= grayR){
            stop_point();
            break;
        }
    }
}