File content as of revision 44:0b92f72641d7:

#include <cmath>

#include "mbed.h"
#include "rtos.h"

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

Mutex mutex;

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

volatile char userInput[256];
volatile bool commandReady = false;
ParseResult parseResult;
volatile bool readyForCommand = true;

void rotateWith(float r, float v) {}

void setVelocity(float v) {}
void playTunes(const vector<float>& tunes) {}

void serialThread() {
  while(true) {
    if (readyForCommand) {
      scanf("%s", userInput);
      ParseResult curr = parse((char *) userInput);
      if (curr.success) {
        mutex.lock();
        commandReady = true;
        parseResult = curr;
        mutex.unlock();
      }
    }
  }
}

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;
Ticker motorOutTicker;
volatile float currentRevs = 0.0;           //number of revs done
volatile float goalRevs = 10.0;             //number of revs to do
volatile float prevError = 0.0;             //previous error in position
volatile double dError = 0.0;               
volatile float currentError = 0.0;          //current error in position

#define kp 0.012f
#define kd 0.02f
#define k 10.0f

const float VKp = 0.5f;
const float VKi = 0.0f;
const float VKd = 0.0f;
volatile float vPrevError = 0.0f;
volatile float targetV = 300.0f;
//given in ms,used to call the PD controller
const float dtControl = 0.002f;

// Period of giving power to the motor
const float dtMotor = 0.005f;

volatile bool commandFinished = false;

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

void controlVelocity() {
    float vErr = 
}

void controlPosition(){
    if (w3 > 300) {                         //restrict the motor speed to 300
        lead = 2;                           // rad/s
        return;
    }
    prevError = currentError;
    currentRevs = diskPosition / 360 + count_i3;        // angle/360 + #of revs
    currentError = goalRevs - currentRevs;              //P term
//    if (currentError < 0.5f) {
//        printf("Reached final position: %f\n\r", currentRevs);
//        commandFinished = true;
//        return;
//    }
    dError = (currentError - prevError)/dtControl;             //D term
    duty = k*(kp*currentError + kd*dError);             //Control motor duty
    if (duty > 0) {
        lead = -2;
    } else {                                  //if duty < 0, reverse motor spin           
        lead = 2;                           // direction to decelerate
        duty = -duty;
    }
}

void rotate(float r) {
    printf("Rotating for %f revolutions\n\r", r);
    goalRevs = r;
    state = updateState();
    // Kickstart
    motorOut((state-orState+lead+6)%6, 0.3f);
    controlTicker.attach(&controlPosition, dtControl);
    motorOutTicker.attach(&runMotor, dtMotor);
}

void i1rise(){
    state = updateState();
    //motorOut((state-orState+lead+6)%6, duty);
    
    if (I3.read() == 1) {                   //Only count revolutions if the
        count_i3++;                         // rotor spins forward
    }    
}
//TODO merge with i_edge by measuring angular velocity in i1rise.
void i3rise(){
    state = updateState();
//    motorOut((state-orState+lead+6)%6, duty);
    
    w3 = angle/dt_I3.read();                //Calc angular velocity
    
    dt_I3.reset();
}

void i_edge(){                              //Upon status led interrupt, update
   state = updateState();                   // the motor output
//   motorOut((state-orState+lead+6)%6, duty);
}
//Todo: add comments on this fucntion
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();
}

void runCommand(const ParseResult& command);

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

    
    I1.rise(&i1rise);                   //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);                //Assign interrupt handlers for
    CHA.fall(&CHA_fall);                // precision angle LEDs
    CHB.rise(&CHB_rise);
    CHB.fall(&CHB_fall);

//    state = updateState();
//    // Kickstart
//    motorOut((state-orState+lead+6)%6, 0.3f);
//    controlTicker.attach(&controlPosition, dtControl);
//    motorOutTicker.attach(&runMotor, dtMotor);
    printf("Ready\n\r");
    while (true) {
      if (count_i3 >= goalRevs && !commandFinished) commandFinished = true;
//        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);
//        printf("dError: %f , currentError: %f, prevError: %f, currentRevs: %f\n\r",dError, currentError, prevError, currentRevs);
    // Work with user input here
      if (commandReady) {
//        printf("Got command: %d\n\r", parseResult.mode);
        commandReady = false;
        readyForCommand = false;
        commandFinished = false;
        runCommand(parseResult);
      }
      if (commandFinished && !readyForCommand) {
        motorOutTicker.detach();
        stopMotor();                        //Turn off the motor if position is reached
        readyForCommand = true;
        printf("Ready\n\r");
      }
    }
    return 0;
}

void runCommand(const ParseResult& command) {
  int mode = command.mode;
  switch(mode) {
    case 0: rotateWith(command.rotations, command.velocity); break;
    case 1: rotate(command.rotations); break;
    case 2: setVelocity(command.velocity); break;
    case 3: playTunes(command.tunes); break;
    default: return;
  }
}