File content as of revision 11:99b63117ffdd:

#include "mbed.h"

#define KP 5.8  //Pゲイン

//#define blinky

DigitalOut ledCheck( PTE2); //動作check
DigitalOut ledCount( PTB11);    //プログラムの切り替え確認
BusOut ledCHG( PTB10, PTE3);    //change_gray用
BusOut ledLL( PTE5, PTE4);  //ロボットから進行方向を見て左（わざと逆書き）
BusOut ledRR( PTB8, PTB9);  //ロボットから進行方向を見て右
AnalogIn sensorR( PTC2);
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 whiteCL = 0.265, grayCL = 0.15, blackCL = 0.03;
float whiteCR = 0.235, grayCR = 0.13, blackCR = 0.03;
float whiteL = 0.1, grayL = 0.06, blackL = 0.013;
float whiteR = 0.1, grayR = 0.06, blackR = 0.013;
float sensor[4];    //sensor[0]:sensorL ... sensor[3]:sensorR
float pr, pl;

void stop_point();
void motor_check();

void set_sensor(){      //センサ系
    sensor[0] = sensorL.read();
    sensor[1] = sensorCL.read();
    sensor[2] = sensorCR.read();
    sensor[3] = sensorR.read();
}

void go_straight_p(){   //P制御でトレース
    pl = (sensor[1] - grayCL) / (whiteCL - grayCL);
    pr = (sensor[2] - grayCR) / (whiteCR - grayCR);
    Mrighti = 2;
    if( pl >= 0.0)
        Mrightp = (KP * pr + 0.3) * 1.0f;
    else
        Mrightp = 1.0f;
    Mlefti = 2;
    if( pr >= 0.0)
        Mleftp = (KP * pl + 0.3) * 1.0f;
    else
        Mleftp = 1.0f;
    ledRR = 0b01;
    ledLL = 0b01;
}

void go_straight_CR(){  //CRのみでトレース
    pr = (sensor[2] - grayCR) / (whiteCR - grayCR);
    Mrighti = 2;
    Mrightp = (KP * pr + 0.5) * 1.0f;
    Mlefti = 2;
    Mleftp = (1.5 - KP * pr) * 1.0f;
    ledRR = 0b11;
    ledLL = 0;
}

void turn_right(){
    Mrighti = 1;
    Mrightp = 0.05f;
    Mlefti = 2;
    Mleftp = 0.5f;
    ledRR = 0b01;
    ledLL = 0;
}

void turn_left(){
    Mrighti = 2;
    Mrightp = 0.5f;
    Mlefti = 1;
    Mleftp = 0.05f;
    ledRR = 0;
    ledLL = 0b01;
}

void turn_right_corner( float f){
    motor_check();
    wait(f);
    sensor[3] = sensorR.read();
    while( sensor[3] >= blackR){
        sensor[3] = sensorR.read();
        turn_right();
    }
    stop_point();
//    turn_left();
//    wait(0.3);
}

void turn_right_corner_ver2( float f){
    motor_check();
    wait(f);
    sensor[2] = sensorCR.read();
    while( sensor[2] >= blackCR){
        sensor[2] = sensorCR.read();
        turn_right();
    }
    stop_point();
//    turn_left();
//    wait(0.3);
}

void turn_left_corner( float f){
    motor_check();
    wait(f);
    sensor[0] = sensorL.read();
    while( sensor[0] >= blackL){
        sensor[0] = sensorL.read();
        turn_left();
    }
    stop_point();
//    turn_right();
//    wait(0.3);
}

void turn_left_corner_ver2( float f){
    motor_check();
    wait(f);
    sensor[1] = sensorCL.read();
    while( sensor[1] >= blackCL){
        sensor[1] = sensorCL.read();
        turn_left();
    }
    stop_point();
//    turn_right();
//    wait(0.3);
}

void motor_check(){   //モータドライバの調子の確認用と単純に直進
    Mrighti = 2;
    Mrightp = 1.0f;
    Mlefti = 2;
    Mleftp = 1.0f;
}

void stop_point(){
    if( Mrighti == 1)
        Mrighti = 2;
    else if( Mrighti == 2)
        Mrighti = 1;
    if( Mlefti == 1)
        Mlefti = 2;
    else if( Mlefti == 2)
        Mlefti = 1;
    Mrightp = 1.0f;
    Mleftp = 1.0f;
    wait(0.1);
    Mrighti = 0;
    Mlefti = 0;
    ledRR = 0b11;
    ledLL = 0b11;
}

void LED_blinky(){      //プログラムの切り替わり確認用
    for( int i = 0; i < 4; i++){
        ledCount = !ledCount;
        wait(0.05);
    }
}

int main() {
    ledCheck = 1;
    LED_blinky();
    while(1){      //スタート～段差～最初の左折カーブ
        set_sensor();
        if( sensor[3] <= blackR){
            turn_right_corner_ver2( 0.2);
            break;
        } else
            go_straight_p();
    }
    #ifdef blinky
    LED_blinky();
    #endif
    while(1){       //左折カーブ～段差～左折カーブ
        set_sensor();
        if( sensor[0] <= blackL){
            turn_left_corner_ver2( 0.2);
            break;
        } else
            go_straight_p();
    }
    #ifdef blinky
    LED_blinky();
    #endif
    while(1){       //左折カーブ～左折カーブ
        set_sensor();
        if( sensor[0] <= blackL){
            turn_left_corner_ver2( 0.2);
            break;
        } else
            go_straight_p();
    }
    #ifdef blinky
    LED_blinky();
    #endif
    while(1){       //左折カーブ～トンネル手前のマーク
        set_sensor();
        if( sensor[0] <= grayL && sensor[3] <= grayR){
            wait(0.2);
            break;
        } else
            go_straight_CR();
    }
    #ifdef blinky
    LED_blinky();
    #endif
    while(1){       //トンネル手前のマーク～Ｔ字
        set_sensor();
        if( sensor[0] <= blackL && sensor[3] <= blackR){
            turn_right_corner_ver2( 0.2);
            break;
        } else
            go_straight_p();
    }
    #ifdef blinky
    LED_blinky();
    #endif
    while(1){       //Ｔ字～半円～鋭角カーブ
        set_sensor();
        if( sensor[0] <= blackL){
            turn_left_corner_ver2( 0.5);
            break;
        } else
            go_straight_CR();
    }
    #ifdef blinky
    LED_blinky();
    #endif
    while(1){       //鋭角カーブ終わり～ゴール
        set_sensor();
        if( sensor[0] <= blackL)
            turn_left_corner_ver2( 0.2);
        else if( sensor[3] <= blackR)
            turn_right_corner_ver2( 0.2);
        else if( sensor[0] <= grayL && sensor[3] <= grayR){
            stop_point();
            break;
        } else
            go_straight_p();
    }
    #ifdef blinky
    LED_blinky();
    #endif
}