File content as of revision 0:be99ed42340d:

/**
 * Drive a robot forwards or backwards by using a PID controller to vary
 * the PWM signal to H-bridges connected to the motors to attempt to maintain
 * a constant velocity.
 */

#include "Motor.h"
#include "QEI.h"
#include "PID.h"

Motor leftMotor(p21, p20, p19);  //pwm, inB, inA
Motor rightMotor(p22, p17, p18); //pwm, inA, inB
QEI leftQei(p29, p30, NC, 624);  //chanA, chanB, index, ppr
QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr
//Tuning parameters calculated from step tests;
//see http://mbed.org/cookbook/PID for examples.
PID leftPid(0.4312, 0.1, 0.0, 0.01);  //Kc, Ti, Td
PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td

int main() {

    leftMotor.period(0.00005);  //Set motor PWM periods to 20KHz.
    rightMotor.period(0.00005);

    //Input and output limits have been determined
    //empirically with the specific motors being used.
    //Change appropriately for different motors.
    //Input  units: counts per second.
    //Output units: PwmOut duty cycle as %.
    //Default limits are for moving forward.
    leftPid.setInputLimits(0, 3000);
    leftPid.setOutputLimits(0.0, 0.9);
    leftPid.setMode(AUTO_MODE);
    rightPid.setInputLimits(0, 3200);
    rightPid.setOutputLimits(0.0, 0.9);
    rightPid.setMode(AUTO_MODE);


    int leftPulses      = 0; //How far the left wheel has travelled.
    int leftPrevPulses  = 0; //The previous reading of how far the left wheel
    //has travelled.
    float leftVelocity  = 0.0; //The velocity of the left wheel in pulses per
    //second.
    int rightPulses     = 0; //How far the right wheel has travelled.
    int rightPrevPulses = 0; //The previous reading of how far the right wheel
    //has travelled.
    float rightVelocity = 0.0; //The velocity of the right wheel in pulses per
    //second.
    int distance     = 6000; //Number of pulses to travel.

    wait(5); //Wait a few seconds before we start moving.

    //Velocity to mantain in pulses per second.
    leftPid.setSetPoint(1000);
    rightPid.setSetPoint(975);

    while ((leftPulses < distance) || (rightPulses < distance)) {

        //Get the current pulse count and subtract the previous one to
        //calculate the current velocity in counts per second.
        leftPulses = leftQei.getPulses();
        leftVelocity = (leftPulses - leftPrevPulses) / 0.01;
        leftPrevPulses = leftPulses;
        //Use the absolute value of velocity as the PID controller works
        //in the % (unsigned) domain and will get confused with -ve values.
        leftPid.setProcessValue(fabs(leftVelocity));
        leftMotor.speed(leftPid.compute());

        rightPulses = rightQei.getPulses();
        rightVelocity = (rightPulses - rightPrevPulses) / 0.01;
        rightPrevPulses = rightPulses;
        rightPid.setProcessValue(fabs(rightVelocity));
        rightMotor.speed(rightPid.compute());

        wait(0.01);

    }

    leftMotor.brake();
    rightMotor.brake();

}