File content as of revision 48:02d5565b8fac:

//#include <cmath>
#include <vector>

#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;
 
//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;
//}

const float VKp = 0.23f;       //0.054, 1.1, 0.012
const float VKi = 0.0085f;    //0.0054, 0.01
const float VKd = 0.02f;    //0.0135, 0.02, 0.012

volatile float targetV = 150.0f;
volatile float totalVErr = 0.0f;
volatile float vPrevErr = 0.0f;
volatile float vErr = 0.0f;
volatile double dErr = 0.0f;

const float dtControl = 0.01f;
//Thread::wait(0.1) in the main for calling motorOut

void controlVelocity(){
    if(w3 > targetV*2)
        w3 = targetV;
    vErr = targetV - w3;
    dErr = (vErr - vPrevErr)/dtControl;
    totalVErr += vErr*dtControl;
    velocityDuty = VKp*vErr + VKi*totalVErr + VKd*dErr;
    if(velocityDuty < 0){
        velocityDuty = -velocityDuty;
        lead = 2;
    }
    else
        lead = -2;
    vPrevErr = vErr;
}

//Timer profiler;
//volatile float profilerDt = 0; 

//takes 11-12us
void i3rise(){
//  profiler.reset();
//  profiler.start();
    state = updateState();
//    motorOut((state-orState+lead+6)%6, velocityDuty);
    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 i_edge(){
    state = updateState();
//    motorOut((state-orState+lead+6)%6, velocityDuty);
}
 
//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];
//}
//
//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 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() {
//    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();
    
    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);
   
    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);

    controlTicker.attach(&controlVelocity, dtControl);  
//    printTicker.attach(&interruptPrint, 1.0f);
    state = updateState();  
    motorOut((state-orState+lead+6)%6, 1.0f);                     //Kickstart the motor
    wait(0.1);

//    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(1);
        motorOut((state-orState+lead+6)%6, velocityDuty);
        print("Speed: %f, duty cycle: %f, currentRevs: %i, vErr %f, prevErr; %f, dErr %f, totalVErr: %f \n\r",w3, velocityDuty, count_i3, VKp*vErr,VKp*vPrevErr, dErr, VKi*totalVErr);

//        wait(0.1);
//        if(currentRevs >= goalRevs){
//            controlTicker.detach();
////            pwmPeriod.detach();
//            stopInterrupts();
//            stopMotor(/*(int)diskPosition*/);
//        }

    }
    return 0;
}