File content as of revision 1:246e8a504681:

/**
 PID Controller for maintaining constant velocity for robots using incremental encoder
 */

#include "Motor.h"
#include "PID.h"
#include "IncrementalEncoder.h"

IncrementalEncoder leftQei(p29);
IncrementalEncoder rightQei(p30);
Motor leftMotor(p23, p6, p5); // pwm, fwd, rev
Motor rightMotor(p22, p8, p7); // pwm, fwd, rev

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     = 600; //Number of pulses to travel.

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

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

    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.readTotal();
        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.readTotal();
        rightVelocity = (rightPulses - rightPrevPulses) / 0.01;
        rightPrevPulses = rightPulses;
        rightPid.setProcessValue(fabs(rightVelocity));
        rightMotor.speed(rightPid.compute());

        wait(0.01);

    }

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

}