File content as of revision 45:bfd7cbd41957:

#include <cmath>

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

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

//#define print(...) sprintf((char*)userOutput, __VA_ARGS__); outputRequested = true

Mutex mutex;
Thread motorControlThread;

Ticker monitorTicker;
void monitor();

volatile float w3 = 0;                  //Angular velocity
volatile float duty = 0.2;
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 playTunes(const vector<float>& tunes) {}

volatile char userOutput[256];
volatile bool outputRequested=false;

//void serialOut() {
//    while (true) {
//        if (outputRequested) {
////            mutex.lock();
//            printf("%s\n\r", userOutput);
//            outputRequested = false;
////            mutex.unlock();
//        }
//    }
//}
//void runMotor() {
//    while (true) {
//        motorOut((state-orState+lead+6)%6, duty);
//        Thread::wait(5);
//    }
//    print("On");
//}

//void motorSwitchOn() {
//    motorControlThread.start(callback(runMotor));  
//}
//
//void motorSwitchOff() {
//    motorControlThread.terminate();
//}


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;
Ticker controlTicker;
Ticker motorOutTicker;
Ticker lifeTicker;

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



//given in ms,used to call the PD controller
const float dtControl = 0.2f;
// Period of giving power to the motor
const float dtMotor = 0.05f;

volatile bool commandFinished = false;

float min(float a, float b) {
    return a<b ? a : b;
}

float computeDelta(float p) {
    float dMin = 0.1;
    float dMax = 0.4;

    if (p > dMax)
        return dMax;
    else if (p > dMin)
        return p;
    else
        return dMin;
}

const float VKp = 1.1f;
const float VKi = 0.01f;
const float VKd = -0.000f;
volatile float vPrevError = 0.0f;
volatile float targetV = 300.0f;
volatile float dutyParam = 0.0f;
volatile float totalVErr = 0.0f;
volatile float vPrevErr = 0.0f;
volatile float vErr = 0;
void controlVelocity() {
    vErr = (targetV - w3) / targetV;
    float dErr = vErr - vPrevErr;
    totalVErr += vErr;
    dutyParam = VKp*vErr + VKi*totalVErr + VKd*dErr;
    duty = computeDelta(dutyParam);
    if (vErr < 0) {
//        dutyParam = 0.1f;
//        lead = -lead;
    }
    vPrevErr = vErr;
//    dutyParam = 2.0f * VKp * vErr;
//    if (dutyParam > 0.0f) {
//        duty = min(1.0f, dutyParam / maxDutyParam);
//    } else {
//        printf("Reversed the motor!\n\r");
//        duty = min(1.0f, -dutyParam / maxDutyParam);
//        lead *= -1;
//    }
//    duty = 0.2f;
}

void stopCommand() {
    commandFinished = true;
    lifeTicker.detach();
    controlTicker.detach();
}

void setVelocity(float v) {
    targetV = v;
    goalRevs = 900.0f;
    printf("Spinning with V=%.2f\n\r", targetV);
    state = updateState();
    motorOut((state-orState+lead+6)%6, 0.3f);
    controlTicker.attach(&controlVelocity, dtControl);
//    motorSwitchOn();
//    motorOutTicker.attach(&runMotor, dtMotor);  
    lifeTicker.attach(&stopCommand, 10);
}

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();
//      }
//    }
//    Thread::wait(10);
//    if (outputRequested) {
        printf("V = %.2f; delta = %.2f dutyParam = %.2f vErr = %.2f vTotalErr= %.2f\n\r", w3, duty, dutyParam, vErr, totalVErr);
//        printf("Current pos: %.2f\n\r", diskPosition);
        Thread::wait(2000);
//        printf("%s\n\r", userOutput);
//        outputRequested = false;
//    }
  }
}
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) {
    float diff = CH_state - CH_state_prev;

    CH_state_prev = CH_state;
//    diskPosition += -diff * angularResolution;
    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() {
    Thread serialHandler;

//    motorControlThread.start(callback(runMotor));
    motorHome();                        //Initialise motor before any interrupt
//    stopMotor();    
    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);

//    printf("Ready\n\r");
    serialHandler.start(callback(serialThread));
    controlTicker.attach(&controlVelocity, dtControl);
//    setVelocity(100);
    while (true) {
        motorOut((state-orState+lead+6)%6, duty);
        // Values of 1 or 10 seem to make no difference for v = 100
        Thread::wait(10);
//        printf("Current pos: %.2f\n\r", diskPosition);
    }
    return 0;
}


//
//void monitor() {
////    print("Current speed %.2f\n\r", w3);
//    print("Current revs: %.2f\n\r", currentRevs);
//}