File content as of revision 1:1afb2ef8c2cc:

/*
memo
grayCR,grayCLを変動させる
grayR,grayLも変動させたほうがいい…？
止まるポイントで電圧値が変化している（していないと困るわけだが）なら変動を見て判断もできる（コメントアウトしてある）
*/

#include "mbed.h"

#define KP 1.5  //Pゲイン
#define CHG 1.0 //change_gray用の係数
#define WHITE_CL 0.39   //初期値
#define WHITE_CR 0.39   //初期値
#define GRAY_CL 0.21    //初期値
#define GRAY_CR 0.32    //初期値
//#define STOP_RATIO 0.06   //止まる判断をする変動率

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 whiteCL[2], whiteCR[2], grayCL[2], grayCR[2]; //[1]が基準値
float blackCL = 0.03;   //閾値（再定義必須）の跡地
float blackCR = 0.26;   //↑と同じ
float whiteL = 0.23, blackL = 0.2, grayL = 0.215;
float whiteR = 0.25, blackR = 0.2, grayR = 0.225;
float sensor[4];    //sensor[0]:sensorL ... sensor[3]:sensorR
float pr, pl;

//float sensorLval[2], sensorRval[2];   //[0]が今，[1]が一個前

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;
    }
}

void go_straight_p(){
    Mrighti = 1;
    Mrightp = (KP * pr) * 1.0f;
    Mlefti = 1;
    Mleftp = (KP * pl) * 1.0f;
    ledR = 0;
    ledL = 0;
}

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

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

void turn_right_ver2(){
    while( sensor[0] > grayL)
        turn_right();
    while( sensor[2] > grayCR[0])
        turn_left();
}

void turn_left_ver2(){
    while( sensor[3] > grayR)
        turn_left();
    while( sensor[1] > grayCL[0])
        turn_right();
}

void go_straight_check(){   //モータドライバの調子の確認用
    Mrighti = 1;
    Mrightp = 0.8f;
    Mlefti = 1;
    Mleftp = 0.8f;
}

void stop_point(){
    Mrighti = 2;
    Mrightp = 0.5f;
    Mlefti = 2;
    Mleftp = 0.5f;
    wait(0.05);
    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]);
    }
    else{   //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]);
    }
}

int main() {
    set_threshold();
    go_straight_check();
    wait(0.2); 
    while(1) {
        sensor[0] = sensorL.read();
        sensor[1] = sensorCL.read();
        sensor[2] = sensorCR.read();
        sensor[3] = sensorR.read();
        
        /*sensorLval[1] = sensorLval[0];
        sensorRval[1] = sensorRval[0];
        sensorLval[0] = sensor[0];
        sensorRval[0] = sensor[3];*/
        
        pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
        pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
        
        if( sensor[1] <= grayCL[0]){    //sensorCL <= grayCL
            turn_left();
            change_gray();
        } else if( sensor[2] <= grayCR[0]){ //sensorCR <= grayCR
            turn_right();
            change_gray();
        } else if( sensor[0] <= grayL && sensor[1] <= grayCL[0]){
            turn_left_ver2();
            change_gray();
        } else if( sensor[3] <= grayR && sensor[2] <= grayCR[0]){
            turn_right_ver2();
            change_gray();
        } else
            go_straight_p();
        //if( ((sensorLval[1] - sensorLval[0]) / sensorLval[1]) > STOP_RATIO && ((sensorRval[1] - sensorRval[0]) / sensorRval[1]) > STOP_RATIO){
        if( sensor[0] <= grayL && sensor[3] <= grayR){
            stop_point();
            break;
        }
    }
}