File content as of revision 3:f1ddc26da601:

#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
#define P_TERM 1
#define I_TERM 0
#define D_TERM 20

// PID terms for collision
#define P_TERM_COL 1
#define I_TERM_COL 0
#define D_TERM_COL 20


int main() {
    m3pi.cls();
    //t_for_music.start();
    m3pi.locate(0,0);
    m3pi.printf("Battery");
    m3pi.locate(0,1);
    float battery = m3pi.battery();
    m3pi.printf("%.0fmV",battery*1000);
    wait(3);
    
    
    // distance sensor
    int reading;
    float time[2];
    m3pi.cls();
    rf.VL6180xInit();
    rf.VL6180xDefautSettings();
    int check = 1;
    int Sensor_num = 0;
    int reading_history[3];
    int i;
    for(i = 0;i<3;i++){
        reading_history[i] = 255;
        }
    float ave_reading;

    
    rf.triggerDistance();
    
    
    m3pi.sensor_auto_calibrate();
    // variables for PID
    float right;
    float left;
    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();

    while(1) {
        Sensor_num = Sensor_num + 1;
        m3pi.cls();
        //t.start();
        //time[0] = t.read();
        col.update();
        bl=col.b;
        gr=col.g;
        re=col.r;
        
        // 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) <= 7000){
            right = speed+power;
            left  = speed-power;
            
            // limit checks
            if (right < MIN)
                right = MIN;
            else if (right > MAX)
                right = MAX;
                
            if (left < MIN)
                left = MIN;
            else if (left > MAX)
                left = MAX;
        }
        
        // get distance once every three times because of the processing speed
        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){
                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){
            right = ((ave_reading - 90)/100) + ((ave_reading - 90)/100) * power_collision;
            left = ((ave_reading - 90)/100) - ((ave_reading - 90)/100) * power_collision;
       // limit checks
            if (right < MIN)
                right = MIN;
            else if (right > MAX)
                right = MAX;
                
            if (left < MIN)
                left = MIN;
            else if (left > MAX)
                left = MAX;
       }
       
       //if light is on its left side, use light to control 
        
        if((gr+re) > 7000 && ave_reading > 190){
           // 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);
                right = light_speed + light_speed*power_collision;
                left = light_speed - light_speed*power_collision;
                
                // limit checks
                if (right < MIN)
                    right = MIN;
                else if (right > MAX)
                    right = MAX;
                    
                if (left < MIN)
                    left = MIN;
                else if (left > MAX)
                    left = MAX;
           // }
        }
       
       
        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();
    }
}