File content as of revision 1:1dd468e2ad40:

#include "mbed.h"
#include "ContinuousServo.h"
#include "Tach.h"
#include "TCS3472_I2C.h"

InterruptIn hall(p21);
BusOut flash(LED1, LED2, LED3, LED4);
TCS3472_I2C rgb_sensor(p9, p10);
ContinuousServo left(p23);                  //Set up left wheel driver
ContinuousServo right(p26);                 //Set up right wheel driver
Tach tleft(p17,64);                         //Set up left tachometer
Tach tright(p13, 64);                       //Set up right tachometer

float wl;                                   //Left wheel velocity
float wr;                                   //Right wheel velocity
float e;                                    //Error

float r = 1.3125;                           //radius of wheels
float l = 4.0625;                           //width of wheel base
float omega = 0.0;                          //angular velocity of wheels
float v = 0.5;                              //translational velocity of wheels
float kp = 1.0;                             //Proportional control gain

float vl = ((omega*l)+(2*v))/(2*r);         //Calculates left wheel velocity
float vr = ((omega*l)-(2*v))/(2*r);         //Calculates right wheel velocity

int rgb_data[4];
float PWMbrightness = 1.0;
float LB = 0.0;
 
void mine() {
    flash = 8.0;
}
 
int main() {
    
    hall.rise(&mine);
    hall.fall(&mine);
    
    rgb_sensor.enablePowerAndRGBC();
    rgb_sensor.setIntegrationTime(100);
    
    while(1) {           // wait around, interrupts will interrupt this!
        left.speed(vl);                     //Move the left wheel
        right.speed(vr);                    //Move the right wheel
        wl = tleft.getSpeed();              //Read the left tachometer
        wr = tright.getSpeed();             //Read the right tachometer
    
        e = wl - (-wr);                     //Calculate the error
        vr = vr - (kp*e);                   //Readjust the right wheel velocity
        
        LB = PWMbrightness;
        rgb_sensor.getAllColors(rgb_data);
        
        if(rgb_data[1]>999 && rgb_data[2]<999 && rgb_data[3]<999){
        flash = 1.0;
        }
        else if(rgb_data[1]<999 && rgb_data[2]>999 && rgb_data[3]<999){
            flash = 2.0;
            }
            else if(rgb_data[1]<999 && rgb_data[2]<999 && rgb_data[3]>999){
                flash = 4.0;
                }
    }
}