File content as of revision 1:66d0178e8f40:

//Satre, Gagnon, Ewing, Batten 
//Tach Initial 

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

Serial pc(USBTX,USBRX); //Establish serial connection

//speed
float wL, wR;
float x = 0.2, y = 0.2, dist, realdist;
 
ContinuousServo left(p23);
ContinuousServo right(p26);
AnalogIn sonar(p19); //range sensor 9.8 mV/in
 
//encoders
Tach tleft(p17,64);
Tach tright(p13,64);

        
int main()
{ 
    
    pc.baud(9600); //set baud rate
    
    while(1) {
        
        left.speed(x);
        right.speed(-y); //right negative to go forward
        x = x;
        y = y;
        
        wL = tleft.getSpeed(); //speed of left wheel (rev/sec)
        wR = tright.getSpeed(); //speed of right wheel (rev/sec)
        dist = sonar.read();
        realdist = dist/0.00277;
            
        if (wL<wR) {
            x = x+0.03;
            y = y; }
        else if (wR<wL) {
            x = x;
            y = y+0.03;}
        else if (wR == wL) {
            x = x;
            y = y;}
        else if ((x || y) > 0.2) {
            x = 0.2;
            y = y;}
            
        if (realdist <= 8) {
            left.stop();
            right.stop();
            }   
        pc.printf("%f, %f\n %f\n %f %f\n", wL, wR, realdist, x, y);
        
        wait(.2);
     
}  //while
}  //main