File content as of revision 1:c70bc01ebfdd:

/**
 * 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.
 */
 /*Sources:     mbed Rover cookbook page: https://developer.mbed.org/cookbook/mbed-Rover
                InterruptIn handbook page: https://developer.mbed.org/handbook/InterruptIn 
 */
 
#include "mbed.h"
#include "motordriver.h"
#include "PID.h"
//one full revolution=193 counts


class Counter {
    public:
    Counter(PinName pin) : _interrupt(pin) {        // create the InterruptIn on the pin specified to Counter
        _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
    }
 
    void increment() {
        _count++;
    }
 
    int read() {
        return _count;
    }
 
private:
    InterruptIn _interrupt;
    volatile int _count;
};

Motor leftMotor(p22, p5, p6, 1); // pwm, fwd, rev, can brake 
Motor rightMotor(p21, p8, p7, 1); // pwm, fwd, rev, can brake
Counter leftPulses(p9), rightPulses (p10);
//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 old
PID leftPid(0.4620, 0.1, 0.0, 0.01);  //Kc, Ti, Td
PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
DigitalOut led(LED1);

int main() {
    //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.8);
    leftPid.setMode(AUTO_MODE);
    rightPid.setInputLimits(0, 3000);
    rightPid.setOutputLimits(0.0, 0.8);
    rightPid.setMode(AUTO_MODE);

    
    int leftPrevPulses  = 0, leftActPulses=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 rightPrevPulses = 0, rightActPulses=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     = 1000; //Number of pulses to travel.
    led=0;

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

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

    while ((leftActPulses < distance) || (rightActPulses < distance)) {

        //Get the current pulse count and subtract the previous one to
        //calculate the current velocity in counts per second.
        leftVelocity = (leftActPulses - leftPrevPulses) / 0.01;
        leftPrevPulses = leftActPulses;
        //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());

        rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
        rightPrevPulses = rightActPulses;
        rightPid.setProcessValue(fabs(rightVelocity));
        rightMotor.speed(rightPid.compute());
        
        wait(0.01);
        led=!led;
        
        leftActPulses=leftPulses.read();
        rightActPulses=rightPulses.read();
    }

    leftMotor.stop(0.5);
    rightMotor.stop(0.5);

}