Diff: main.cpp

Revision:

46:5c50778bb2d5

Parent:

45:bfd7cbd41957

Child:

47:49e6c3fb25dc

diff -r bfd7cbd41957 -r 5c50778bb2d5 main.cpp
--- a/main.cpp	Sun Mar 19 23:46:22 2017 +0000
+++ b/main.cpp	Thu Mar 23 22:45:25 2017 +0000
@@ -1,141 +1,204 @@
 #include <cmath>
-
-#include "mbed.h"
-#include "rtos.h"
+#include <vector>
+//#include "mbed.h"
+//#include "rtos.h"
 
 #include "definitions.h"
 #include "motorControl.h"
 #include "parser.h"
-
-//#define print(...) sprintf((char*)userOutput, __VA_ARGS__); outputRequested = true
+#include "serialHandler.h"
 
 Mutex mutex;
-Thread motorControlThread;
+Thread serialOutputer;
 
-Ticker monitorTicker;
-void monitor();
-
-volatile float w3 = 0;                  //Angular velocity
-volatile float duty = 0.2;
-volatile int count_i3 = 0;
+Thread serialInputer;
 
-volatile char userInput[256];
-volatile bool commandReady = false;
-ParseResult parseResult;
-volatile bool readyForCommand = true;
+#define print(...) sprintf((char*)userOutput, __VA_ARGS__); outputRequested = true;
 
-void rotateWith(float r, float v) {}
-
-void playTunes(const vector<float>& tunes) {}
-
+ParseResult parseResult;
+volatile char userInput[256];
 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");
-//}
+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 motorSwitchOn() {
-//    motorControlThread.start(callback(runMotor));  
-//}
-//
-//void motorSwitchOff() {
-//    motorControlThread.terminate();
-//}
+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 diskPosition = 0.0;  //in degrees
+volatile float velocityDuty = 0.0;
+ 
 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
-
+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.02f
+#define kd 0.019f //0.5f, 0.02
 #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;
+#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;
 }
 
-float computeDelta(float p) {
-    float dMin = 0.1;
-    float dMax = 0.4;
+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();
 
-    if (p > dMax)
-        return dMax;
-    else if (p > dMin)
-        return p;
-    else
-        return dMin;
+}  
+ 
+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];
 }
 
-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;
+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();
@@ -143,148 +206,71 @@
 }
 
 void setVelocity(float v) {
-    targetV = 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);  
+//    print("Spinning with V=%.2f\n\r", targetV);
+
     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 playTune(float freq) {
+    motorPWMPeriod = 1.0f / freq;
+//    motorOut(0, 0.5);
+    Thread::wait(1000);
 }
-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 playTunes(const vector<float>& tunes) {
+    for (int i=0; i<tunes.size(); ++i) {
+        playTune(tunes[i]);
+    }
+//    motorPWMPeriod = defaultMotorPWMPeriod;
+    stopMotor(0);
 }
 
-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
+   
+    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);
 
-//    printf("Ready\n\r");
-    serialHandler.start(callback(serialThread));
-    controlTicker.attach(&controlVelocity, dtControl);
-//    setVelocity(100);
+    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) {
-        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);
+        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;
 }
-
-
-//
-//void monitor() {
-////    print("Current speed %.2f\n\r", w3);
-//    print("Current revs: %.2f\n\r", currentRevs);
-//}
\ No newline at end of file