File content as of revision 3:e33676098294:

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

// Test change

Serial pc(USBTX,USBRX);
//servos
ContinuousServo left(p23);
ContinuousServo right(p26);
//encoders
Tach tLeft(p17,64);
Tach tRight(p13,64);

// speed controller
Ticker spdCtrl;
DigitalOut myled(LED1);

// Function definition Prototypes (declarations)
void ctrCode(); // declare that a separate (other than main) function named "ctrCode" exists


float vl;
float vr; 
float Ts = 0.01;        // Control update period (seconds) (100 Hz equivalent)
float speedL,speedR;
float dcL;
float dcR;
float dcpL;
float dcpR;
float errpL;
float errpR;
float spdErrL;
float spdErrR;
float kp = 0.1;
float ki = 0.01;
float whl_rad = 3.1; // wheel radius in centimeters
float baseL = 11.0; // wheelbase in centimeters
float om = 0.0; // angular velocity
float sp = 0.0; // forward speed in cm/s

int main()
{
    //wait for MATLAB command
    left.speed(0.0);
    right.speed(0.0);
    while(1) {
        spdErrL = 0.0;
        spdErrR = 0.0;
        dcL = 0.0;
        dcR = 0.0;
        errpL = 0.0;
        dcpL = 0.0;
        errpR = 0.0;
        dcpR = 0.0;
        pc.scanf("%f,%f,%f,%f",&sp,&om,&kp,&ki);

        spdCtrl.attach(&ctrCode,Ts);

        for(int i=1; i<=200; i++) {
            
            vl = (2*sp-om*baseL)/(2*whl_rad); // desired left wheel speed in cm/s
            vr = (2*sp+om*baseL)/(2*whl_rad); // desired right wheel speed in cm/s
            
            
            
            wait_ms(100);
            pc.printf("%f,%f,%f,%f\n", speedL,speedR,dcL,dcR);
        }
        
        
        
        spdCtrl.detach();
        left.stop();
        right.stop();
    }// end while(1)
}


// Additional function definitions
void ctrCode() // function to attach to ticker
{
    myled = !myled; // toggle LED 2 to indicate control update

    speedL = tLeft.getSpeed()*2.0*3.14*whl_rad; // in centimeters per second
    speedR = tRight.getSpeed()*2.0*3.14*whl_rad; // in centimeters per second

    // compute error
    spdErrL = vl-speedL;
    spdErrR = vr-speedR;

    // PI controller
    dcL = dcpL+kp*((Ts/2.0*(ki/kp)+1.0)*spdErrL+(Ts/2.0*(ki/kp)-1.0)*errpL); // compute duty cycle for motor using PI control scheme
    dcR = dcpR+kp*((Ts/2.0*(ki/kp)+1.0)*spdErrR+(Ts/2.0*(ki/kp)-1.0)*errpR); // compute duty cycle for motor using PI control scheme

    // enforce duty cycle saturation
    if(dcL>1.0) {
        dcL = 1.0;
    } else if(dcL<0.0) {
        dcL = 0.0;
    }
    if(dcR>1.0) {
        dcR = 1.0;
    } else if(dcR<0.0) {
        dcR = 0.0;
    }

    // motor control
    left.speed(dcL); // first input is the motor channel, second is duty cycle
    right.speed(-dcR); // first input is the motor channel, second is duty cycle

    // Age variables
    errpL = spdErrL; // age speed error variable
    dcpL = dcL; // age duty cycle variable

    errpR = spdErrR; // age speed error variable
    dcpR = dcR; // age duty cycle variable


} // end ctrCode()