File content as of revision 5:c47b952fbd58:

#include "mbed.h"
#include "ContinuousServo.h"
#include "Tach.h"
#include "TCS3472_I2C.h"
//driving
Tach tLeft(p17,64);
Tach tRight(p13,64);

ContinuousServo left(p23);
ContinuousServo right(p26);
AnalogIn sonar(p19);

//color sensor
PwmOut LB(p25); // PWM out signal to power LED on the sensor
TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
Serial pc(USBTX,USBRX); // Establish serial connection with computer

//Hall effect
InterruptIn hall(p21);
DigitalOut led4(LED4);
DigitalOut toggle(p22);
int counter = 0;

//need distance to stop
float distance;

float l;
float r;
float speedL;
float speedR;
float errorL;
float errorR;
float sampling_per;

float PIpwmL(float desired_speed,float speed); 
float PIpwmR(float desired_speed,float speed);
float PropL(float actual_rgb, float desired_rgb);
float PropR(float actual_rgb, float desired_rgb);

int main() {
    hall.mode(PullUp);
    toggle = 1;
    led4 = 0;
    
    rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor
    rgb_sensor.setIntegrationTime(100); // Set integration time of sensor
    int rgb_data[4]; // declare a 4 element array to store RGB sensor readings
    float PWMbrightness = 1.0; // float specifying brightness of LED (between 0.0 and 1.0)
    pc.baud(921600);
 
    while(1) {
        float son[6];
        for (int count = 0;count<6;count++){
            son[count] = sonar;
            wait(0.01);
        }
        
        float a;
        int i;
        int j;
        int n = 6;
        for (i = 0; i < n; i++) 
        {
            for (j = i + 1; j < n; j++)
            {
                if (son[i] > son[j]) 
                {
                    a =  son[i];
                    son[i] = son[j];
                    son[j] = a;
                }
            }
        }
        pc.printf("%f\n",son[2]);
        distance = 10;//inches;
        distance = 0.003*distance - 0.0057;
        wait(0.05);     
        
        LB = PWMbrightness; // set brightness of sensor LED
        rgb_sensor.getAllColors( rgb_data ); // read the sensor to get red, green, and blue color data along with overall brightness
        wait(0.1);
        
        speedL = tLeft.getSpeed();
        speedR = tRight.getSpeed();
        l = PIpwmL(0.3, speedL);
        r = PIpwmR(0.3, speedR); 
        l = l - 0.41;
        r = r - 0.15;
        
       if(hall == 1){
           led4 = 1;   
           wait(0.4);
           toggle = 0;
           wait(0.1);
           toggle = 1;
           led4 = 0;
           counter = counter +1;
           printf("Counter=%d\n",counter);
           }
        if(son[2] > 0.026){
            left.speed(l);
            right.speed(-r);

            if (rgb_data[0] < 1900){//add value for too dark
                float rn = PropR(rgb_data[0],2100);
                rn = rn + r;
                right.speed(-rn);
                }
            else if (rgb_data[0] > 2900){//add value for too light
                float ln = PropL(rgb_data[0],2100);
                left.speed(ln);
                }
            else if (rgb_data[0] < 2900 && rgb_data[0] > 1900){
             right.speed(-r);
            left.speed(l);
            }
            }
        else {
            left.stop();
            right.stop();
            break;
            }
            }
    wait(0.05);
    }


float PIpwmL(float desired_speed,float speed){
    float integral_errorL = 0.0;  
    float sampling_per = 0.05;    
    float errorL = desired_speed - speed;
    integral_errorL += (errorL*sampling_per);   
    float left = (0.07*integral_errorL) + (0.08*errorL) + 0.48;   
    return left;
    }
float PIpwmR(float desired_speed,float speed){
        float integral_errorR = 0.0;
        float sampling_per = 0.05;
        float errorR = desired_speed - speed;
        integral_errorR += (errorR*sampling_per);
        float right = (0.07*integral_errorR) + (0.08*errorR) + 0.25;
        return right;
    }
float PropR(float actual_rgb, float desired_rgb){
        float integral_errorr = 0.0;
        float sampling_periodr = 0.05;
        float errorr = desired_rgb - actual_rgb;
        integral_errorr += (errorr*sampling_periodr);
        float changer = (0.00006*integral_errorr) + (0.00009*errorr);
        return changer;
        }
float PropL(float actual_rgb, float desired_rgb){
        float integral_error = 0.0;
        float sampling_period = 0.05;
        float error = desired_rgb - actual_rgb;
        integral_error += (error*sampling_period);
        float change = (0.00000015*integral_error) + (0.00000017*error)- 0.18;
        return -change;
        }