File content as of revision 0:e019949c4c69:

#include "mbed.h"

#include "mRotaryEncoder.h"
#include "HBridgeMotor.h"
#include "PID.h"

// PID constants
#define RATE  0.2
#define Kc   1.5 // Proportional
#define Ti   1.0 // Integral
#define Td    0.0 // Derivative

DigitalOut led1(LED1);

// Two PID Controllers
PID leftPID(Kc, Ti, Td, RATE);
PID rightPID(Kc, Ti, Td, RATE);

// Two de-bounced encoders
mRotaryEncoder left_enc(p19, p20, p29);
mRotaryEncoder right_enc(p26, p27, p28);

// Two motor/h-bridge objects
HBridgeMotor left_motor(p25, p24);
HBridgeMotor right_motor(p21, p22);

void initializePidControllers(void) {
    leftPID.setInputLimits(-300, 300);
    leftPID.setOutputLimits(-1.0, 1.0);
    leftPID.setMode(AUTO_MODE);

    rightPID.setInputLimits(-300, 300);
    rightPID.setOutputLimits(-1.0, 1.0);
    rightPID.setMode(AUTO_MODE);
}

int main() {
    int left_pulses = 0, right_pulses = 0;
   
    initializePidControllers();
    
    // 40 encoder ticks per second target speed
    leftPID.setSetPoint(40.0);
    rightPID.setSetPoint(40.0);
    
    while (true) {
        // Get velocities
        float left_velocity = float(left_enc.Get() - left_pulses)/RATE;
        float right_velocity = float(right_enc.Get() - right_pulses)/RATE;
        
        // Pass the PID Controller the latest velocity value
        leftPID.setProcessValue(left_velocity);
        rightPID.setProcessValue(right_velocity);
        
        // Query the PID Controllers for the desired power outputs
        // and set the motors to those powers
        left_motor.set(leftPID.compute());
        right_motor.set(rightPID.compute());
        
        // Save the number of encoder ticks for velocity calculation in the next loop
        left_pulses = left_enc.Get();
        right_pulses = right_enc.Get();
        
        // Toggle the LED and wait
        led1 = 1 - led1;
        wait(RATE);
    }
}