File content as of revision 3:6a42c3c3d816:

/*
タームB作戦（プログラムの流れ）
・AからスタートしてB（トンネル手前）で一旦停止
・閾値の再定義（トンネル内の明るさに）←あらかじめ用意しておいて切り替えてもいい?
・トンネル抜けた地点で再定義（ゴール判定用が急激に変化したら再定義か？）
・あとは流れでいいかな？
*/

#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 white_tunnel, black_tunnel, gray_tunnel;
float sensor[4];    //sensor[0]:sensorL ... sensor[3]:sensorR
float kp = 0.3; //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(){
    Mrighti = 2;
    Mrightp = (kp * pr + 0.5) * 1.0f;
    Mlefti = 2;
    Mleftp = (kp * pl + 0.5) * 1.0f;
    ledR = 0;
    ledL = 0;
}

void go_straight_p(){
    if( pr < (gray - black) / (white - black)){
        Mrighti = 1;
        Mrightp = (kp * pr + 0.3) * 1.0f;
        Mlefti = 2;
        Mleftp = (kp * pl + 0.3) * 1.0f;
        ledR = 1;
        ledL = 0;
    } else if( pl < (gray - black) / (white - black)){
        Mrighti = 2;
        Mrightp = (kp * pr + 0.3) * 1.0f;
        Mlefti = 1;
        Mleftp = (kp * pl + 0.3) * 1.0f;
        ledR = 0;
        ledL = 1;
    } else{
        Mrighti = 2;
        Mrightp = (kp * pr + 0.3) * 1.0f;
        Mlefti = 2;
        Mleftp = (kp * pl - 0.1) * 1.0f;
        ledR = 0;
        ledL = 0;
    }
}

void go_straight_p_tunnel(){
    if( pr < (gray_tunnel - black_tunnel) / (white_tunnel - black_tunnel)){
        Mrighti = 1;
        Mrightp = (kp * pr + 0.3) * 1.0f;
        Mlefti = 2;
        Mleftp = (kp * pl + 0.3) * 1.0f;
        ledR = 1;
        ledL = 0;
    } else if( pl < (gray_tunnel - black_tunnel) / (white_tunnel - black_tunnel)){
        Mrighti = 2;
        Mrightp = (kp * pr + 0.3) * 1.0f;
        Mlefti = 1;
        Mleftp = (kp * pl + 0.3) * 1.0f;
        ledR = 0;
        ledL = 1;
    } else{
        Mrighti = 2;
        Mrightp = (kp * pr + 0.3) * 1.0f;
        Mlefti = 2;
        Mleftp = (kp * pl - 0.1) * 1.0f;
        ledR = 0;
        ledL = 0;
    }
}

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

void go_back(){
    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 = 1;
    Mrightp = 0.5f;
    Mlefti = 1;
    Mleftp = 0.5f;
    wait(0.05);
    Mrighti = 0;
    Mlefti = 0;
    ledRR = 2;
    ledLL = 2;
}

void stop_turn_right(){
    Mlefti = 1;
    Mleftp = 0.7f;
    Mrighti = 2;
    Mrightp = 0.7f;
    wait(0.05);
    Mrighti = 0;
    Mlefti = 0;
    ledRR = 2;
    ledLL = 2;
}

void stop_turn_left(){
    Mlefti = 2;
    Mleftp = 0.7f;
    Mrighti = 1;
    Mrightp = 0.7f;
    wait(0.05);
    Mrighti = 0;
    Mlefti = 0;
    ledRR = 2;
    ledLL = 2;
}

int main() {
    go_straight_check();
    wait(0.2); 
    while(i < 2){   
        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);
            
            go_straight_p();
            if( sensor[0] <= black && sensor[3] <= blackR){
                stop_point();
                i++;
                break;
            }
        }
        
        //以下トンネル内部
        if( i < 2){
            turn_right();
            wait(0.2);
            while(1){
                sensor[3] = sensorR.read();
                turn_right();
                if( sensor[3] <= blackR){
                    stop_turn_right();
                    
                    wait(0.5);
                    
                    break;
                }
            }
            black_tunnel = sensorL.read();
            turn_left();
            wait(0.1);
            
            stop_turn_left();
            wait(0.5);
            
            white_tunnel = sensorL.read();
            gray_tunnel = (white_tunnel - black_tunnel) / 2;
            
            //姿勢を直す
            while(1){
                sensor[3] = sensorR.read();
                turn_left();
                if( sensor[3] <= blackR){
                    stop_turn_left();
                    break;
                }
            }
            
            //トンネルに入る
            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);
                
                go_straight_p();
                
                if( sensor[1] < gray_tunnel && sensor[2] < gray_tunnel){
                    stop_point();
                    ledRR = 0b11;   //←２進数表記使える…？
                    wait(0.5);
                    break;
                }
            }
            
            //トンネルの中でのトレース
            while(1){
                sensor[0] = sensorL.read();
                sensor[1] = sensorCL.read();
                sensor[2] = sensorCR.read();
                sensor[3] = sensorR.read();
                
                pr = (sensor[2] - black_tunnel) / (white_tunnel - black_tunnel);
                pl = (sensor[1] - black_tunnel) / (white_tunnel - black_tunnel);
                
                go_straight_p_tunnel();
                
                if( sensor[1] > white || sensor[2] > white){
                    stop_point();
                    ledRR = 0b11;   //←２進数表記使える…？
                    wait(0.5);
                    break;
                }
            }
        }
    }
}