File content as of revision 0:259b726d0807:

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


//create a new quadrature encoder object
QEI encoder(p25, p26, 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 error = 0;
float old_error = 0;
float goal_position = 0;
float p_gain = 0;
float d_gain = 0;
float sample_period = .0004;
float command = 0;

//

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


void updateCommand(float desired_position, float KP, float KD){
    
    position = pulsesToRadians(encoder.getPulses());        //use the getPulses functin to read encoder position, convert value to radians
    old_error = error;                                      
    error = position - desired_position;
    command = KP*error + KD*sample_period*(error-old_error);    //modify the command
    
    
}

void update(void){                                  //The function attached to the ticker can't take inputs
    updateCommand(goal_position, p_gain, d_gain);
}



int main() {
    

    SampleFrequency.attach(&update, sample_period);        //Encoder will be sampled at 2.5 kHz
    
    while(1) {
        
    }
}