File content as of revision 3:8d12cc699e4c:

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

Serial pc(USBTX,USBRX);// color sensor
TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
DigitalIn hall(p21);// Hall effect sensor
DigitalOut hallpwr(p22); 
AnalogIn sonar(p19); // sonar sensor, range sensor 9.8 mV/inch

//servos
ContinuousServo left(p23);
ContinuousServo right(p26);

//encoders
Tach tLeft(p17,64);
Tach tRight(p13,64);

// functions
float calcPWM(float desired_velocity, float velocity);

// initialize variables
    w = 0;
    v = 0.5;
    r = 3.3;
    phi = 90;
    l = 4.1;
    float left_rps;
    float right_rps;
    Vright = (2v+ wl)/2r;   // rad/s
    Vleft = (2v - wl)/2r;   // rad/s

void forward(){
    
}
 
void stop(){
    
}

int main()
{


//drive straight

    while(1) {
        
    right = Vright; 
    left = Vleft;
    right_rps = tRight.read() * 2* pi; //rad/s
    left_rps = tLeft.read() * 2 * pi;  //rad/s
    sDC_right = calcPWM(Vright,right_rps); // SET DUTY CYCLE FOR RIGHT SERVO
    sDC_left = calcPWM(Vleft,left_rps); // SET DUTY CYCLE FOR LEFT SERVO
    right.speed(sDC_right);
    left.speed(sDC_left);
    wait(0.05);
    



    }
}


// calculates the PWM duty cycle given the desired and measured velocity
float calcPWM(float desired_velocity, float velocity)
{

    float v_err;
    // velocity error
    v_err = (desired_velocity - velocity);

    sDC = .039*v_err; // enter duty cycle calculation here!!
    return sDC;
}