File content as of revision 38:e57ffbd9b75d:

#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;

volatile int CHA_state = 0x00;
volatile int CHB_state = 0x00;
volatile int CH_state = 0x00;
volatile int CH_state_prev = 0x00;

volatile float diskPosition = 0.0;  //in degrees

Timer dt_I3;
Timer motorTimer;
Ticker controlTicker;

volatile float currentRevs = 0.0;                 //number of revs done
volatile float goalRevs = 999.0;
volatile float prevError = 0.0;
volatile double dError = 0.0;
volatile float currentError = 0.0;

#define kp 0.009f
#define ki 0.0f //0.05f
#define kd 0.025f //0.5f
#define k 10.0f
#define dt 0.002f                        //given in ms, used to call the PID c.

void control(){
    if (w3 > 300) {
        lead = 2;
        return;
    }
    prevError = currentError;
    currentRevs = diskPosition / 360 + count_i3;    // 1/360degr + 2pi*revs
    currentError = goalRevs - currentRevs;
    dError = (currentError - prevError)/dt;
    duty = k*(kp*currentError + kd*dError);
    if (duty > 0) lead = -2;
    else {
        lead = 2;
        duty = -duty;
    }
}

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 updateDiskPosition() {
  if (CH_state != CH_state_prev) {
    int diff = CH_state - CH_state_prev;
    
    CH_state_prev = CH_state;
    if (abs(diff) == 1 || abs(diff) == 3) {
        if (diff < 0)
            diskPosition += angularResolution;
        else
            diskPosition -= angularResolution;
    } 
    else if (abs(diff) == 2) {
        if (diff < 0)
            diskPosition += 2.0f * angularResolution;
        else
            diskPosition -= 2.0f * angularResolution;
    }

    if (diskPosition >= 360.0f) {
      diskPosition -= 360.0f;
    } else if (diskPosition < -360.0f) {
      diskPosition += 360.0f;
    }
  }
}

void updateRelativeState() {
  CH_state = relativeStateMap[CHB_state + 2*CHA_state];
}

void CHA_rise() {
  CHA_state = 1;
  updateRelativeState();
  updateDiskPosition();
}
void CHA_fall() {
  CHA_state = 0;
  updateRelativeState();
  updateDiskPosition();
}
void CHB_rise() {
  CHB_state = 1;
  updateRelativeState();
  updateDiskPosition();
}
void CHB_fall() {
  CHB_state = 0;
  updateRelativeState();
  updateDiskPosition();
}

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+1) {
        //pc.printf("Speed: %f, duty cycle: %f, revs done: %d \n\r",w3, duty, count_i3);
        pc.printf("Speed: %f, duty cycle: %f, revs done: %d, dError: %f , currentError: %f, prevError: %f, currentRevs: %f \n\r",w3, duty, count_i3, dError, currentError, prevError, currentRevs);
        //pc.printf("Disk position: %f \n\r",fi0);
    }
    stopMotor();
    return 0;
}