File content as of revision 18:12598db37e38:

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

#define kp 0.75f
#define ki 0.5f
#define kd 1.0f
#define dt 0.02f                        //given in ms, used to call the PID c.

volatile uint8_t state = 0;
volatile float w3 = 0;                  //Angular velocity
volatile float duty = 0.40;
volatile int count_i3 = 0;
const float angularVelocities[29] = {0, 0, 0, 56.3705020, 153.953598, 
221.162308, 436.561981, 652.034058, 669.472534, 671.117249, 703.662231, 
704.767273, 695.868835, 676.609924, 689.303345, 685.318481, 680.420166, 
681.527283, 683.529175, 700.758423, 742.759216, 737.354797, 733.224365, 
716.746521, 714.952209, 717.483154, 723.597839, 727.788757, 727.957336};

const float dutyCycles [29] = { 0, 0.05, 0.10, 0.15, 0.20, 0.25, 0.30, 0.35, 
                                0.36, 0.37, 0.38, 0.39, 0.40, 0.41, 0.42, 0.43, 
                                0.44, 0.45, 0.50, 0.55, 0.60, 0.65, 0.70, 0.75, 
                                0.80, 0.85, 0.90, 0.95, 1};
const float angle = 6.283;              //2*pi for 1 revolution
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 goalW = 0;               //desired angular velocity
volatile float accError = 0;
volatile float prevError = 0;

float getDuty(float w){
    for (int i = 0; i < 28; i++) {      //iterate through the angular velocities
        if (w > angularVelocities[i] && w <= angularVelocities[i+1]) {
            if (w-angularVelocities[i] < angularVelocities[i+1]-w ) {
                return dutyCycles[i];
            }
            else {
                return dutyCycles[i+1];
            }
        }
    }
    return 0;
}

void control(){
    fi0 = 6.283 * count_i3;             //fi0 = 2*pi*revs
    float error = fi - fi0;
    accError += error*dt;
    float dError = (prevError - error)/dt;
    goalW = kp*error + ki*accError + kd*dError;
    prevError = error;
    duty = getDuty(goalW);
}

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, 1.0f);            //Kickstart the motor
    
    while (count_i3 <= goalRevs) {

        pc.printf("Speed: %f, duty cycle: %f, revs done: %d \n\r",w3, duty, count_i3);
        /*
        if(duty < 0.00f) {
            stopMotor();
            return 0;    
        }
        
        if(motorTimer.read() >= 30) {
            stopMotor();
            return 0;    
        }
        */
    }
    stopMotor();
    return 0;
}