File content as of revision 4:edec6fe32ba9:

#include "mbed.h"

#define KP 5.8  //Pゲイン
#define CHG 0.05 //change_gray用の係数
#define WHITE_CL 0.92   //初期値
#define WHITE_CR 0.92    //初期値
#define GRAY_CL 0.46    //初期値
#define GRAY_CR 0.5     //初期値
#define WHITE_L 0.1     //初期値
#define WHITE_R 0.1     //初期値
#define GRAY_L 0.05     //初期値
#define GRAY_R 0.05     //初期値

//#define sensor_not_straight
//#define graychange

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[2], whiteCR[2], grayCL[2], grayCR[2]; //[1]が初期設定の基準値，[0]が変動する閾値
float whiteL[2], whiteR[2], grayL[2], grayR[2];
float blackCL = 0.18, blackCR = 0.1;   //閾値
float blackL = 0.028, blackR = 0.028;
float stepL = 0.45, stepR = 0.38;
float sensor[4];    //sensor[0]:sensorL ... sensor[3]:sensorR
float pr, pl;

void stop_point_ver2();
void motor_check();

void set_threshold(){   //閾値の初期設定
    for( int i = 0; i < 2; i++){
        whiteCL[i] = WHITE_CL;
        whiteCR[i] = WHITE_CR;
        grayCL[i] = GRAY_CL;
        grayCR[i] = GRAY_CR;
        whiteL[i] = WHITE_L;
        whiteR[i] = WHITE_R;
        grayL[i] = GRAY_L;
        grayR[i] = GRAY_R;
    }
}

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[0]) / (whiteCL[0] - grayCL[0]);
    pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
    Mrighti = 2;
    Mrightp = (KP * pr) * 1.0f;
    Mlefti = 2;
    Mleftp = (KP * pl) * 1.0f;
    ledRR = 0b01;
    ledLL = 0b01;
}

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

void go_straight_CL(){  //CLのみでトレース
    pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
    Mrighti = 2;
    Mrightp = (1.3 - KP * pl) * 1.0f;
    Mlefti = 2;
    Mleftp = (KP * pl + 0.3) * 1.0f;
    ledRR = 0b01;
    ledLL = 0b11;
}

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(){
    #ifdef sensor_not_straight
    while( sensor[2] > blackCR){
        set_sensor();
        go_straight_p();
    }
    #endif
    motor_check();
    wait(0.2);
    while( sensor[3] > blackR){   //sensorR > grayR[0]
        sensor[3] = sensorR.read();
        turn_right();
    }
    stop_point_ver2();
    turn_left();
    wait(0.3);
}

void turn_left_corner(){
    #ifdef sensor_not_straight
    while( sensor[1] > blackCL){
        set_sensor();
        go_straight_p();
    }
    #endif
    motor_check();
    wait(0.2);
    while( sensor[0] > blackL){   //sensorL > grayL[0]
        sensor[0] = sensorL.read();
        turn_left();
    }
    stop_point_ver2();
    turn_right();
    wait(0.3);
}

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

void stop_point_ver2(){
    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 change_gray(){
    if( sensor[1] <= grayCL[0]){    //sensorCL <=grayCL
        whiteCR[0] = sensorCR.read();
        grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
        grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
        grayR[0] = grayR[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
        grayL[0] = grayL[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
        ledRR = 0b01;
        ledLL = 0;
    }
    else if( sensor[2] <= grayCR[0]){   //sensorCR <= grayCR
        whiteCL[0] = sensorCL.read();
        grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
        grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
        grayR[0] = grayR[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
        grayL[0] = grayL[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
        ledRR = 0;
        ledLL = 0b01;
    }
}

void change_gray_ver2(int i){
    if( i == 0){        //Rが白
        whiteR[0] = sensorR.read();
        grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteR[1] - whiteR[0]) / whiteR[1]);
        grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteR[1] - whiteR[0]) / whiteR[1]);
        grayR[0] = grayR[1] * ( 1 + CHG * (whiteR[1] - whiteR[0]) / whiteR[1]);
        grayL[0] = grayL[1] * ( 1 + CHG * (whiteR[1] - whiteR[0]) / whiteR[1]);
        ledCHG = 0b01;
        wait( 0.1);
        ledCHG = 0;
    }
    else {      //Lが白
        whiteL[0] = sensorL.read();
        grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteL[1] - whiteL[0]) / whiteL[1]);
        grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteL[1] - whiteL[0]) / whiteL[1]);
        grayR[0] = grayR[1] * ( 1 + CHG * (whiteL[1] - whiteL[0]) / whiteL[1]);
        grayL[0] = grayL[1] * ( 1 + CHG * (whiteL[1] - whiteL[0]) / whiteL[1]);
        ledCHG = 0b10;
        wait( 0.1);
        ledCHG = 0;
    }
}

void go_step(){
    if( sensor[0] > stepL){     //左側に段差がある場合
        #ifdef sensor_not_straight
        motor_check();
        wait(0.2);
        #endif
        while( sensor[3] < stepR){
            set_sensor();
            go_straight_CR();
        }
    } else if( sensor[3] > stepR){      //右側に段差がある場合
        #ifdef sensor_not_straight
        motor_check();
        wait(0.2);
        #endif
        while( sensor[0] < stepL){
            set_sensor();
            go_straight_CL();
        }
    } else
        go_straight_p();        
}

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

int main() {
    #ifdef graychange
    int i = 1;      //i == 0ならRがwhite，i == 1ならLがwhite
    #endif
    ledCheck = 1;
    set_threshold();
    LED_blinky();
    motor_check();
    wait(0.2);
    while(1){      //スタート～最初の左折カーブ
        set_sensor();
        #ifdef graychange
        change_gray_ver2(i);
        #endif
        if( sensor[3] <= blackR){
            turn_right_corner();
            break;
        } else
            go_straight_p();
    }
    LED_blinky();
    while(1){       //左折カーブ～段差～左折カーブ
        set_sensor();
        if( sensor[0] <= blackL){
            turn_left_corner();
            break;
        } else
            go_step();
    }
    LED_blinky();
    #ifdef graychange
    i = 0;
    #endif
    while(1){       //左折カーブ～左折カーブ
        set_sensor();
        #ifdef graychange
        change_gray_ver2(i);
        #endif
        if( sensor[0] <= blackL){
            turn_left_corner();
            break;
        } else
            go_straight_p();
    }
    LED_blinky();
    while(1){       //左折カーブ～トンネル手前のマーク
        set_sensor();
        if( sensor[0] <= blackL && sensor[3] <= blackR)
            break;
        else
            go_straight_CR();
    }
    LED_blinky();
    while(1){       //トンネル手前のマーク～Ｔ字
        set_sensor();
        if( sensor[0] <= blackL && sensor[3] <= blackR){
            turn_right_corner();
            break;
        } else
            go_straight_p();
    }
    LED_blinky();
    #ifdef graychange
    i = 0;
    #endif
    while(1){       //Ｔ字～半円終わり
        set_sensor();
        #ifdef graychange
        change_gray_ver2(i);
        #endif
        if( sensor[0] <= blackL){
            turn_left_corner();
            break;
        } else
            go_straight_p();
    }
    LED_blinky();
    while(1){       //半円終わり～ゴール
        set_sensor();
        if( sensor[0] <= blackL)
            turn_left_corner();
        else if( sensor[3] <= blackR)
            turn_right_corner();
        else if( sensor[0] <= blackL && sensor[3] <= blackR){
            stop_point_ver2();
            break;
        } else
            go_straight_p();
    }
    LED_blinky();
}

//        change_gray_ver2( i);
/*        if( sensor[0] <= blackL){
            turn_left_corner();
//            change_gray();
        } else if( sensor[3] <= blackR){
            turn_right_corner();
//            change_gray();
        } else*/
//            go_straight_p();
//          go_straight_CR();
//          go_straight_CL();
/*以下メモ
・段差
        if( sensor[0] > 閾値){
            wait(0.5);  //片輪上がるまで待ちたい
            sensor[2] = read.sensorCR;
            go_straight_CR();
        } else if( sensor[3] > 閾値){
            wait(0.5)   //片輪上がるまで待ちたい
            sensor[1] = read.sensorCL;
            go_straight_CL();
        }

・プログラムの切り替え
回転動作をしたら（完了したら）カウント
⇒回転を一発で決める必要有
*/