File content as of revision 1:be893bda5f00:

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

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

ContinuousServo left(p23);
ContinuousServo right(p26);
AnalogIn sonar(p19);
float distance;

float l;
float r;
float speedL;
float speedR;
float errorL;
float errorR;
float sampling_per;

Serial pc(USBTX,USBRX);

float PIpwmL(float desired_speed,float speed); 
float PIpwmR(float desired_speed,float speed); 

int main() {
    while(1) {
        float check;
        check = sonar;
        //son[0] = sonar;
        //son[1] = sonar;
        //son[2] = sonar;
        //son[3] = sonar;
        //son[4] = sonar;
        //son[5] = sonar;
        //float check;
        //check = son[0] + son[1] + son[2] + son[3]+son[4]+son[5];
        //check = check/6;
        check = check*3.3;
        check = check/0.0098;
        pc.printf("%f\n",check);
        distance = 7;//inches;
        wait(0.05);
        if (check > distance){
        
                speedL = tLeft.getSpeed();
                speedR = tRight.getSpeed();
                l = PIpwmL(0.3, speedL);
                r = PIpwmR(0.3, speedR); 

                left.speed(l);
                right.speed(-r);
        
                pc.printf("%f,%f\n",speedL,speedR);
                wait(0.05);
        }
        else {
            left.stop();
            right.stop();
            break;
            }
    }
}
float PIpwmL(float desired_speed,float speed)
{
    float integral_errorL = 0.0;  
    float sampling_per = 0.05;    
    float errorL = desired_speed - speed;
    integral_errorL += (errorL*sampling_per);   
    float left = (0.07*integral_errorL) + (0.08*errorL) + 0.485;   
    return left;
    }
float PIpwmR(float desired_speed,float speed)
{
        float integral_errorR = 0.0;
        float sampling_per = 0.05;
        float errorR = desired_speed - speed;
        integral_errorR += (errorR*sampling_per);
        float right = (0.07*integral_errorR) + (0.08*errorR) + 0.25;
        return right;
    }