File content as of revision 35:5857105c9956:

#include "mbed.h"
#include "rtos.h"
#include "definitions.h"
#include "motorControl.h"
#include "parser.h"
#include <cmath>

volatile float w3 = 0;                  //Angular velocity
volatile float duty = 0.5;
volatile int count_i3 = 0;

Timer dt_I3;
Timer motorTimer;
Ticker controlTicker;

volatile float fi0 = 0;                 //number of revs done
volatile int goalRevs = 50;
volatile float fi = 2*3.1415*goalRevs;
volatile float accError = 0;
volatile float prevError = 0;

#define kp 0.0016f
#define ki 0.0f //0.05f
#define kd 0.00006f //0.5f
#define dt 0.002f                        //given in ms, used to call the PID c.

void control(){
    fi0 = 6.283 * count_i3;             //fi0 = 2*pi*revs
    float error = fi - fi0;
    accError += error*dt;
    float dError = (error - prevError)/dt;
    duty = kp*error + ki*accError + kd*dError;
    prevError = error;
    //if (abs(goalW) > w3) duty = 1 - w3/abs(goalW);
    //else duty = abs(goalW) / w3;
    //duty = getDuty(abs(goalW));
    if (duty > 0) lead = -2;
    else lead = 2;
    //if (duty > 0) lead = -2;
    //else lead = 2;
}

void i3rise(){
    state = updateState();
    motorOut((state-orState+lead+6)%6, duty);
    
    w3 = angle/dt_I3.read();            //Calc angular velocity
    
    dt_I3.reset();
    count_i3++;
}

void i_edge(){
   state = updateState();
   motorOut((state-orState+lead+6)%6, duty);
}  

void CHA_rise(){
}
void CHA_fall(){
}
void CHB_rise(){
}
void CHB_fall(){
}

int main() {
    motorHome();                        //Initialise motor before any interrupt
    
    dt_I3.start();                      //Start the time counters for velocity

    controlTicker.attach(&control, dt);
    
    I1.rise(&i_edge);                   //Assign interrupt handlers for LEDs
    I1.fall(&i_edge);
    I2.rise(&i_edge);
    I2.fall(&i_edge);
    I3.rise(&i3rise);
    I3.fall(&i_edge);
    
//    CHA.rise(&CHA_rise);
//    CHA.fall(&CHA_fall);
//    CHB.rise(&CHB_rise);
//    CHB.fall(&CHB_fall);

    state = updateState();
    motorTimer.start();
    motorOut((state-orState+lead+6)%6, 0.3f);            //Kickstart the motor

    while (count_i3 <= goalRevs) {
        pc.printf("Speed: %f, duty cycle: %f, revs done: %d \n\r",w3, duty, count_i3);
    }
    while (abs(w3) > 10){
        pc.printf("Speed: %f, duty cycle: %f, revs done: %d \n\r",w3, duty, count_i3);
    }
    stopMotor();
    return 0;
}