Diff: main.cpp

Revision:

47:49e6c3fb25dc

Parent:

46:5c50778bb2d5

Child:

48:02d5565b8fac

--- a/main.cpp	Thu Mar 23 22:45:25 2017 +0000
+++ b/main.cpp	Fri Mar 24 14:48:27 2017 +0000
@@ -5,49 +5,49 @@
 
 #include "definitions.h"
 #include "motorControl.h"
-#include "parser.h"
-#include "serialHandler.h"
+//#include "parser.h"
+//#include "serialHandler.h"
 
-Mutex mutex;
-Thread serialOutputer;
-
-Thread serialInputer;
+//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;
+//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();
+//        }
+//    }
+//}
 
-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");
-            }
-        }
-    }
-}
+//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;
@@ -62,79 +62,112 @@
  
 Timer dt_I3;
 Ticker controlTicker;
-Ticker pwmPeriod;
-Ticker pwmTon;
-Ticker motorTicker;
-float dtMotor = 0.1;
+//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;
+//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;
+//#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++;
-}
+//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 startVelocityPwm(float velocityPeriod){
-    pwmPeriod.attach(&velocityPwmPeriod, velocityPeriod);
-}
- 
-void control(){
-    if (w3 > 300) {
+//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.9f;       //0.054, 1.1, 0.012
+const float VKi = 0.001f;    //0.0054, 0.01
+const float VKd = 0.05f;    //0.0135, 0.02
+
+volatile float targetV = 300.0f;
+volatile float totalVErr = 0.0f;
+volatile float vPrevErr = 0.0f;
+volatile float vErr = 0.0f;
+volatile float dErr = 0.0f;
+
+const float dtControl = 0.01f;
+//Thread::wait(0.1) in the main for calling motorOut
+
+void controlVelocity(){
+    vErr = targetV - w3;
+    dErr = (vErr - vPrevErr)/dtControl;
+    totalVErr += vErr*dtControl;
+    velocityDuty = VKp*vErr + VKi*totalVErr + VKd*dErr;
+    if(velocityDuty < 0){
+        velocityDuty = -velocityDuty;
         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;
+    else
+        lead = -2;
+    vPrevErr = vErr;
 }
 
-Timer profiler;
-volatile float profilerDt = 0; 
+//Timer profiler;
+//volatile float profilerDt = 0; 
+
+//takes 11-12us
 inline void i3rise(){
-  profiler.reset();
-  profiler.start();
-    
+//  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();
+//  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) {
@@ -162,7 +195,7 @@
   }
 }
  
-inline void updateRelativeState() {
+void updateRelativeState() {
   CH_state = relativeStateMap[CHB_state + 2*CHA_state];
 }
 
@@ -188,88 +221,72 @@
   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();
-}
+//inline void motorControl(){
+//    if(pwmOut == 1)                         //Only control
+//        precisionMotorOut(360-(((int)diskPosition+phaseLead)%360));
+//}
 
-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);
-}
-
-
+//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() {
+    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);
    
-    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;
+//    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
+//    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
+    motorOut(4, 0.4f);                        //Kickstart the motor
+    controlTicker.attach(&controlVelocity, dtControl);    
+//    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);
-//    
+//    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);
-        }
+//        Thread::wait(1000);
+//        motorOut((state-orState+lead+6)%6, velocityDuty);
+        pc.printf("Speed: %f, duty cycle: %f, currentRevs: %i, vErr %f, dErr %f, totalVErr: %f \n\r",w3, velocityDuty, count_i3, vErr, dErr, totalVErr);
+        wait(0.1);
+//        if(currentRevs >= goalRevs){
+//            controlTicker.detach();
+////            pwmPeriod.detach();
+//            stopInterrupts();
+//            stopMotor(/*(int)diskPosition*/);
+//        }
 
     }
     return 0;