File content as of revision 0:7d319151aaa0:

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

DigitalIn hall(p21);
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 counter = 0.0;
 
void mine() {
    flash = 16.0;
}
 
int main() {
    
    hall.rise(&mine);
    hall.fall(&mine);
    
    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
        
        if(rgb_sensor){
        flash = counter;
        }
    }
}