File content as of revision 5:0998ea3869ff:

#include "mbed.h"
#include "QEI.h"  //QEI is a quadrature encoder library
#define pi  3.14159265358979323846


//create a new quadrature encoder object
QEI encoder(PTB6, PTB7, NC, 1200, QEI::X4_ENCODING);       //(encoder channel 1 pin, encoder channel 2 pin, index (n/a here), counts per revolution, mode (X4, X2))

//Ticker SampleFrequency;

//Values that will be used for the PID controller

int steps_per_rev = 1200; //Encoder CPR * gear ratio
float position = 0;
float p_error = 0;
float old_error = 0;
float goal_position = 0;
float p_gain = 1.0;
float d_gain = 0;
float sample_period = 0.01;
float pwm_period = 0.0002;
float command = 0;

PwmOut Motor = PTB13;       //h-bridge pwm pins
DigitalOut dir1 = PTA0;    //h-bridge direction 1 pins
DigitalOut dir2 = PTA8; 

float pulsesToRadians(float pulses) //Encoder steps to revolutions of output shaft in radians
{
    return ((pulses/steps_per_rev)*(2*pi));
}

void signal_to_hbridge( float signal)   //Input of -1 means full reverse, 1 means full forward
{                                       //An input of magnitude > 1 gets reduced to 1 (or -1) by the pwm.write function
    if (signal < 0) {
        dir1.write(0);
        dir2.write(1);
        Motor.write(signal * -1);
    } 
    else if (signal > 0) {
        dir1.write(1);
        dir2.write(0);
        Motor.write(signal);
    } 
    else {
        dir1.write(0);
        dir2.write(0);
    }
}

float updateCommand(void){
    
    position = pulsesToRadians(encoder.getPulses());        //use the getPulses functin to read encoder position, convert value to radians
    old_error = p_error;                                      
    p_error = position - goal_position;
    return p_gain*p_error + d_gain*(p_error-old_error)/sample_period;    //modify the command based on position feedback
    
}

DigitalOut led1(LED1);
DigitalOut led2(LED2);

int main() {
    led1 = 1;
    led2 = 1;
    //led3 = 1;
    
    wait(0.5);
    led1 = 0;
    led2 = 1;
    
    Timer timer;
    timer.start();
    
    Motor.period(pwm_period);
    
    wait(0.5);
    led1 = 1;
    led2 = 0;
    
    bool state = false;
    long count = 0;
    
    while(1) {
         if (timer.read() > 0.0004) {
              command = updateCommand();
              timer.reset();
              if(++count == 250) {
                  count = 0;
                  state = !state;
                  led2 = state;
              }
         }
         signal_to_hbridge(command);
    }
}