File content as of revision 4:0e6997707f43:

#include "VL6180X.h"
#include "mbed.h"
#include "m3pi.h"
#include "S11059.h"

//必要なオブジェクトを作る
VL6180x rf(p28, p27); //I2C sda and scl
Serial pc(USBTX, USBRX); //USB serial
S11059 col(p28,p27);
m3pi m3pi;
Timer t;

// Minimum and maximum motor speeds
#define MAX 1 //速度の最大値
#define MIN 0 //速度の最小値（バックはしません）

// PID terms ライントレースに必要なPID項
//このプログラムでは車両の下に存在する黒いテープの車両の中心からのズレを状態として、PIDで制御します。
#define P_TERM 1
#define I_TERM 0
#define D_TERM 20

// PID terms 車両が衝突回避を行っているときのライントレースのPID項（速度がおそくなるから値を変えてます）
#define P_TERM_COL 1
#define I_TERM_COL 0
#define D_TERM_COL 20

//リニア信号機，前方車両，ライントレースから計算される速度から車両の速度（左右のタイヤにかかる電圧）を決定するための最も遅い速度のインデックスを返す関数
int min_index(float a,float b,float c,float d,float e,float f){
    float sum1 = a + b;
    float sum2 = c + d;
    float sum3 = e + f;

    if(sum1<sum2 && sum1<sum3){
        return 1;
    }else if(sum1>sum2 && sum3>sum2){
        return 2;
    }else{
        return 3;
    }
}

int main() {
    m3pi.cls();
    //t_for_music.start();
    //一秒間スクリーンに文字を表示する
    m3pi.locate(0,0); //指定した位置（0,0）に
    m3pi.printf("Battery"); //指定した文字"Battery"を表示する
    m3pi.locate(0,1);
    float battery = m3pi.battery();
    m3pi.printf("%.0fmV",battery*1000);
    wait(1);
    
    
    // distance sensor
    int reading; //前方車両との距離を格納する変数
    float time[2]; //サンプリング時間を一定にするための配列
    //おまじない
    m3pi.cls(); 
    rf.VL6180xInit();
    rf.VL6180xDefautSettings();
    
    int check = 1; //サンプリリング時間が乱れた時にシステムを止めるための変数
    int Sensor_num = 0;//距離センサは取得に時間がかかるため3ステップに一回値を取得することにするので、そのための値
    int reading_history[3]; //ノイズがあるため前方車両との距離は最新の3つの値の平均をとるのでそのための配列
    int i;
    //
    for(i = 0;i<3;i++){
        reading_history[i] = 255;//初期値はセンサの最大値
        }
    float ave_reading = 255;

    //おまじない
    rf.triggerDistance();
    m3pi.sensor_auto_calibrate();
    
    
    // variables for PID
    //左右のタイヤにかかる電圧を格納する変数
    float right;//右のタイヤの最終的な電圧（下記の三つの値から最終的にきめる）
    float left;//左のタイヤの最終的な電圧（下記の三つの値から最終的にきめる）
    
    float right1;//ライントレースからの右のタイヤの電圧
    float left1;//ライントレースからの左のタイヤの電圧
    float right2;//前方車両から計算される右のタイヤの電圧
    float left2;//前方車両から計算される左のタイヤの電圧
    float right3;//リニア信号機から受け取る右のタイヤの電圧
    float left3;//リニア信号機から受け取る左のタイヤの電圧
    
    int mode = 1;
    
    
    //ライントレースに必要な変数
    float current_pos_of_line = 0.0;//現在の車両のライン上のポジション
    float previous_pos_of_line = 0.0;//過去の車両のライン上のポジション
    float derivative,proportional,integral = 0;
    float power;
    float power_collision;
    float speed = MAX;
    
    
    //for color sensor
    int bl=0;
    int gr=0;
    int re=0;   

    t.start();//サンプリング時間を一定に保つためタイマーを使い時間を計るので、タイマーをスタートさせる。
    time[0] = t.read();//スタート時間を格納する

    while(1) {
        
        Sensor_num = Sensor_num + 1;//距離センサは取得に時間がかかるため3ステップに一回値を取得することにするので、そのための値
        m3pi.cls();
        //t.start();
        //time[0] = t.read();
        col.update();
        //カラーセンサからのRGB値をそれぞれの変数に格納する。
        bl=col.b;
        gr=col.g;
        re=col.r;
        
        //左右のモータの電圧を初期化
        right1 = MAX;
        right2 = MAX;
        right3 = MAX;
        left1 = MAX;
        left2 = MAX;
        left3 = MAX;
        
        //ライントレースするための左右のモータの電圧の計算
        // Get the position of the line.
        current_pos_of_line = m3pi.line_position();        
        proportional = current_pos_of_line;
        
        // Compute the derivative
        derivative = current_pos_of_line - previous_pos_of_line;
        
        // Compute the integral
        integral += proportional;
        
        // Remember the last position.
        previous_pos_of_line = current_pos_of_line;
        
        // Compute the power
        power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
        power_collision = (proportional * (P_TERM_COL) ) + (integral*(I_TERM_COL)) + (derivative*(D_TERM_COL)) ;
        
        // Compute new speeds if there is nothing ahead   
        if(ave_reading >= 190 && (gr+re) <= 10000){
            right1 = speed+power;
            left1  = speed-power;
            
            // limit checks
            if (right1 < MIN)
                right1 = MIN;
            else if (right1 > MAX)
                right1 = MAX;
            if (left1 < MIN)
                left1 = MIN;
            else if (left1 > MAX)
                left1 = MAX;
        }
        
        // get distance once every three times because of the processing speed
        //3ステップに一回、車両は前方車両との距離を取得する。
        if((Sensor_num %3)== 1){    
            reading = rf.pollDistance();
            rf.triggerDistance();
            if(reading < 90 && reading > 0.1){
                /*m3pi.locate(0,0);
                m3pi.printf("%dmm",reading);
                wait(10);*/
                reading = 90;
            }else if(reading < 0.1){//たまにセンサの不具合により距離0を返すときがあるのでそれを除外。
                reading = 255;
                }
            reading_history[0] = reading_history[1];
            reading_history[1] = reading_history[2];
            reading_history[2] = reading;
            ave_reading = float(reading_history[0]+reading_history[1]+reading_history[2])/3;//三回取得した平均値を前方車両との距離とする
        }
        
       // if distance is too close, change the speed
       //前方車両との距離から左右のモータの電圧を計算
        if(ave_reading < 190){
            right2 = ((ave_reading - 90)/100) + ((ave_reading - 90)/100) * power_collision;
            left2 = ((ave_reading - 90)/100) - ((ave_reading - 90)/100) * power_collision;
       // limit checks
            if (right2 < MIN)
                right2 = MIN;
            else if (right2 > MAX)
                right2 = MAX;
                
            if (left2 < MIN)
                left2 = MIN;
            else if (left2 > MAX)
                left2 = MAX;
       }
       
       //if light is on its left side, use light to control 
        //カラーセンサから得られるRGB値から左右のモータの電圧を計算する。
        if((gr+re) > 10000){
           // if(Sensor_num%3 == 2){
                double light_position = (((double)gr)/((double) (re+gr)));
                double light_speed = light_position*(10.0/7.0)-(2.0/7.0);
                right3 = light_speed + light_speed*power_collision;
                left3 = light_speed - light_speed*power_collision;
                
                // limit checks
                if (right3 < MIN)
                    right3 = MIN;
                else if (right3 > MAX)
                    right3 = MAX;
                    
                if (left3 < MIN)
                    left3 = MIN;
                else if (left3 > MAX)
                    left3 = MAX;
           // }
        }
        
        //上記で計算された左右のモータにかかる電圧（３つ）から最終的な電圧を計算する。
        mode = min_index(right1,left1,right2,left2,right3,left3);
        
        if(mode == 1){//（ライントレースから求められた値）
            right = right1;
            left = left1;
        }else if(mode == 2){//（前方車両との距離から求められた値）
            right = right2;
            left = left2;
        }else{//（カラーセンサのRGB値より求められた値）
            right = right3;
            left = left3;
        }
        
        //求められた左右のモータの電圧を入力
        m3pi.left_motor(left);
        m3pi.right_motor(right);
        //wait_ms(1);
        
        //サンプル時間が一定に保たれているかチェック
        while(t.read() - time[0] < 0.005){
            check = 0;
            }
        if(check == 1){//保たれていなければストップ
            m3pi.locate(0,0);
            m3pi.printf("%dmm",reading);
            wait(1);
            m3pi.stop();
            }
        check = 1;
        
        //タイマーストップ
        t.stop();
        //タイマースタート（次のステップのサンプル時間をチェックするため）
        t.start();
        time[0] = t.read();//（スタート時間をチェック）
    }
}