File content as of revision 2:f4d6c9ba05d0:

/**
 * 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"
#include "uLCD_4DGL.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;
};

int distTransform(float input) {
    if (input>3) return 6;
    else if (input>2.5) return 8;
    else if (input>2) return 10;
    else if (input>1.5) return 14;
    else if (input>1.1) return 22;
    else if (input>0.9) return 27;
    else if (input>0.75) return 35;
    else if (input>0.6) return 45;
    else if (input>0.5) return 60;
    else if (input>0.4) return 75;
    else return 99;    
}

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.4310, 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);
AnalogIn ain(p15);
uLCD_4DGL uLCD(p28,p27,p29); // serial tx, serial rx, reset pin;

int main() {
    uLCD.printf("\nHello World\n"); //Default Green on black text
    
    //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, 3200);
    rightPid.setOutputLimits(0.0, 0.8);
    rightPid.setMode(AUTO_MODE);
    Serial pc(USBTX, USBRX);
    
    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 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     = 1000; //Number of pulses to travel.
    led=0;

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

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

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

        leftVelocity = (leftPulses.read() - leftPrevPulses) / 0.01;
        leftPrevPulses = leftPulses.read();
        rightVelocity = (rightPulses.read() - rightPrevPulses) / 0.01;
        rightPrevPulses = rightPulses.read();

        leftPid.setProcessValue(fabs(leftVelocity));
        leftMotor.speed(leftPid.compute());
        rightPid.setProcessValue(fabs(rightVelocity));
        rightMotor.speed(rightPid.compute());
        
        pc.printf("\n%i", distTransform(ain));
        
        uLCD.locate(1,2);
        uLCD.printf("%i", distTransform(ain));
        wait(0.1);
        led=!led;
        
    }

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

}