File content as of revision 1:da5831bec373:

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

//servos
ContinuousServo left(p23);
ContinuousServo right(p26);
//ecnoders
Tach tLeft(p17,64);
Tach tRight(p13,64);
float proportion(float);
float wL;
float wR;
float ref=1.45;
float err;
float lspeed=1;
float newspeed;
float Kp=0.1;
int main()
{
    while(1) {
        left.speed(lspeed);
        wL=tLeft.getSpeed();
        err=ref-wL;
        if (err!=0)  {
            newspeed=proportion(err);
            left.speed(newspeed);
        }
        printf("wL=%f, err=%f, speed=%f\n\r",wL,err,newspeed);
        wait(0.2);
    }
}
float proportion(float err)
{
    return newspeed=Kp*err;
}
// left.speed(0.49);
//wL=tLeft.getSpeed();
//right.speed(-0.5); //the servo is inverted so much be negative
//wR=tRight.getSpeed();
//printf("Both moving, l=%f, r=%f\n\r",wL,wR);
//wait(0.5);
//left.stop();
//right.stop();

//}
//}