File content as of revision 4:dc705df93090:

#include "mbed.h"
#include "rtos.h"
#include "definitions.h"
#include "motorControl.h"

#define kp 0.75
#define ki 0.5
#define kd 1
#define dt 0.02 //given in ms, used to call a ticker

volatile uint8_t state = 0;
volatile uint8_t orState = 0;                   //Motor rotor offset.
volatile float w [3] = {0, 0, 0};               //Angular velocities
volatile float avgW = 0;
volatile int duty = 100;
volatile int count_i3 = 0;

const uint16_t angle = 6283;                    //2*pi*1000 for 1 revolution
Timer dt_I1;
Timer dt_I2;
Timer dt_I3;
Timer motorTimer;
Ticker controlTicker;

volatile float fi0 = 0;     //number of revs done
volatile int goalRevs = 20;
volatile float fi = 2*3.1415*goalRevs;
volatile float goalW = 0;      //desired angular velocity
volatile int accError = 0;

/*
void control(){
    fi0 = 6.283 * count_i3; //fi0 = 2*pi*revs
    int error = fi - fi0;
    accError += error;
    dError = (prevError - error)/dt;    //prev error needs to be stored
    goalW = kp*error + ki*accError + kd*dError;
}
*/

void i1rise(){
    state = updateState();
    motorOut((state-orState+lead+6)%6, duty);
    
    dt_I1.stop();
    w[0] = angle/dt_I1.read_ms();                //Calc angular velocity
    
    dt_I1.reset();
    dt_I1.start();                       
}

void i2rise(){
    state = updateState();
    motorOut((state-orState+lead+6)%6, duty);
    
    dt_I2.stop();
    w[1] = angle/dt_I2.read_ms();
    
    dt_I2.reset();
    dt_I2.start();                       
}

void i3rise(){
    state = updateState();
    motorOut((state-orState+lead+6)%6, duty);
    
    dt_I3.stop();
    w[2] = angle/dt_I3.read_ms();
    
    dt_I3.reset();
    dt_I3.start();
    count_i3++;                   
}

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

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

int main() {
    //Probably measure orState from hardware and make it a const?
    orState = motorHome();  //Initialise motor before any interrupt
    
    dt_I1.start();          //Start the time counters for velocity
    dt_I2.start();          //Probably put these in an init function?
    dt_I3.start();
    
    motorOut(4, 100);            //Kickstart the motor
    motorTimer.start();
    
    //controlTicker.attach(&control, dt);
    
    I1.rise(&i1rise);       //Assign interrupt handlers for LEDs
    I1.fall(&i_fall);
    I2.rise(&i2rise);
    I2.fall(&i_fall);
    I3.rise(&i3rise);
    I3.fall(&i_fall);
//    CHA.rise(&CHA_rise);
//    CHA.fall(&CHA_fall);
//    CHB.rise(&CHB_rise);
//    CHB.fall(&CHB_fall);

    while (1) {
        /*
        const int rotations = 10;
        pc.printf("Rotation: %d ",count_i3);
        if (count_i3 == rotations) {
            stopMotor();
            return 0;
        }
        */
        /*
        avgW = (w[0] + w[1] + w[2])/3;    //average speeds for better prediction
        pc.printf("Speed: %f, duty cycle: %d ; ",w[2], duty);
        duty ++;
        wait(2);
        if(duty > 100) {
            stopMotor();
            return 0;    
        }
        */
        //if(motorTimer.read_ms() >= 10000) {
        //    stopMotor();
        //    return 0;    
        //}
    }
}