File content as of revision 46:5c50778bb2d5:

#include <cmath>
#include <vector>
//#include "mbed.h"
//#include "rtos.h"

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

Mutex mutex;
Thread serialOutputer;

Thread serialInputer;

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

ParseResult parseResult;
volatile char userInput[256];
volatile char userOutput[256];
volatile bool outputRequested=false;

volatile bool commandReady = false;
volatile bool readyForCommand = true;
void serialOut() {
    while(true) {
        if (outputRequested) {
            printf("%s\n\r", userOutput);
            mutex.lock();
            outputRequested = false;
            mutex.unlock();
        }
    }
}

void serialIn() {
    while(true) {
        if (readyForCommand) {
            scanf("%s", userInput);
            ParseResult curr = parse((char *) userInput);
            if (curr.success) {
                mutex.lock();
                commandReady = true;
                parseResult = curr;
                mutex.unlock();
                printf("Got command\n\r");
            }
        }
    }
}

volatile float w3 = 0;                  //Angular velocity
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
volatile float velocityDuty = 0.0;
 
Timer dt_I3;
Ticker controlTicker;
Ticker pwmPeriod;
Ticker pwmTon;
Ticker motorTicker;
float dtMotor = 0.1;
 
volatile float currentRevs = 0.0;                 //number of revs done
volatile float goalRevs = 130.5;
volatile float prevError = 0.0;
volatile double dError = 0.0;
volatile float currentError = 0.0;
volatile int phaseLead = 120;
 
//Make variables constant?
#define kp 0.012f
#define kd 0.019f //0.5f, 0.02
#define k 10.0f
#define dt 0.002f                        //given in ms, used to call the PID c.
volatile float velocityPeriod = 0.004;     //0.4ms (velocityPwm) >> 40us (motorPwm)
volatile float velocityTon = 0.0;
volatile bool pwmOut = 1;
volatile int debugCounter = 0;
 
inline void velocityPwmTon(){
    pwmOut = 0;
    pwmTon.detach();
}
 
inline void velocityPwmPeriod(){
    pwmOut = 1;
    pwmTon.attach(&velocityPwmTon, velocityTon);
//    debugCounter++;
}
 
void startVelocityPwm(float velocityPeriod){
    pwmPeriod.attach(&velocityPwmPeriod, velocityPeriod);
}
 
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;
    velocityDuty = k*(kp*currentError + kd*dError);
    if (velocityDuty > 0) lead = -2;
    else {
        lead = 2;
        velocityDuty = -velocityDuty;
    }
    if(velocityDuty > 1)
        velocityPeriod = 1;
   
    velocityTon = velocityPeriod * velocityDuty;
}

Timer profiler;
volatile float profilerDt = 0; 
inline void i3rise(){
  profiler.reset();
  profiler.start();
    
    w3 = angle/dt_I3.read();                //Calc angular velocity    
    dt_I3.reset();
 
    if (I2.read() == 1)                     //Only count revolutions if the
        count_i3++;                         // rotor spins forward
  profiler.stop();
  profilerDt = profiler.read_us();

}  
 
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;
    }
  }
}
 
inline void updateRelativeState() {
  CH_state = relativeStateMap[CHB_state + 2*CHA_state];
}

inline void CHA_rise() {
  CHA_state = 1;
  updateRelativeState();
  updateDiskPosition();
}
// Takes 5-6us
inline void CHA_fall() {
  CHA_state = 0;
  updateRelativeState();
  updateDiskPosition();
}
inline void CHB_rise() {
  CHB_state = 1;
  updateRelativeState();
  updateDiskPosition();
}
inline void CHB_fall() {
  CHB_state = 0;
  updateRelativeState();
  updateDiskPosition();
}
 
inline void motorControl(){
    if(pwmOut == 1)                         //Only control
        precisionMotorOut(360-(((int)diskPosition+phaseLead)%360));
}

void rotateWith(float r, float v) {}


Ticker lifeTicker;

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

void setVelocity(float v) {
//    targetV = v;
    goalRevs = 900.0f;
//    print("Spinning with V=%.2f\n\r", targetV);

    lifeTicker.attach(&stopCommand, 10);
}

void playTune(float freq) {
    motorPWMPeriod = 1.0f / freq;
//    motorOut(0, 0.5);
    Thread::wait(1000);
}

void playTunes(const vector<float>& tunes) {
    for (int i=0; i<tunes.size(); ++i) {
        playTune(tunes[i]);
    }
//    motorPWMPeriod = defaultMotorPWMPeriod;
    stopMotor(0);
}



int main() {
    I3.rise(&i3rise);
   
    LPins[0] = &L1L;                        //Define the pins for the pin array
    LPins[1] = &L1H;
    LPins[2] = &L2L;
    LPins[3] = &L2H;
    LPins[4] = &L3L;
    LPins[5] = &L3H;
   
    motorHome();
   
    dt_I3.start();                          //Start the time count for velocity
//    controlTicker.attach(&control, dt);
    motorTicker.attach(&motorControl, dtMotor); //Call motor control periodicly
   
    CHA.rise(&CHA_rise);
    CHA.fall(&CHA_fall);
    CHB.rise(&CHB_rise);
    CHB.fall(&CHB_fall);

    precisionMotorOut(360-120);             //Kickstart motor with 120deg
//    pwmPeriod.attach(&velocityPwmPeriod, velocityPeriod);
    
    serialOutputer.start(callback(serialOut));
    serialOutputer.set_priority(osPriorityLow);
    serialInputer.start(callback(serialIn));
    serialInputer.set_priority(osPriorityLow);
//    
    
    while (true) {
        Thread::wait(1000);
//        print("Current pos: %.2f\n\r", diskPosition);

        print("Speed: %f, duty cycle: %f, currentRevs: %i commandsGiven: , %f\n\r",w3, velocityDuty, count_i3, profilerDt);
        if(currentRevs >= goalRevs){
            controlTicker.detach();
            pwmPeriod.detach();
            stopInterrupts();
            stopMotor((int)diskPosition);
        }

    }
    return 0;
}