Diff: main.cpp

Revision:

44:0b92f72641d7

Parent:

43:8b6b4040e635

Child:

45:bfd7cbd41957

--- a/main.cpp	Sat Mar 18 15:59:37 2017 +0000
+++ b/main.cpp	Sat Mar 18 21:35:45 2017 +0000
@@ -19,7 +19,7 @@
 volatile bool readyForCommand = true;
 
 void rotateWith(float r, float v) {}
-void rotate(float r) {}
+
 void setVelocity(float v) {}
 void playTunes(const vector<float>& tunes) {}
 
@@ -48,7 +48,7 @@
 Timer dt_I3;
 Timer motorTimer;
 Ticker controlTicker;
-
+Ticker motorOutTicker;
 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
@@ -58,9 +58,29 @@
 #define kp 0.012f
 #define kd 0.02f
 #define k 10.0f
-#define dt 0.002f                           //given in ms,used to call the PD c.
+
+const float VKp = 0.5f;
+const float VKi = 0.0f;
+const float VKd = 0.0f;
+volatile float vPrevError = 0.0f;
+volatile float targetV = 300.0f;
+//given in ms,used to call the PD controller
+const float dtControl = 0.002f;
+
+// Period of giving power to the motor
+const float dtMotor = 0.005f;
 
-void control(){
+volatile bool commandFinished = false;
+
+void runMotor() {
+    motorOut((state-orState+lead+6)%6, duty);
+}
+
+void controlVelocity() {
+    float vErr = 
+}
+
+void controlPosition(){
     if (w3 > 300) {                         //restrict the motor speed to 300
         lead = 2;                           // rad/s
         return;
@@ -68,18 +88,34 @@
     prevError = currentError;
     currentRevs = diskPosition / 360 + count_i3;        // angle/360 + #of revs
     currentError = goalRevs - currentRevs;              //P term
-    dError = (currentError - prevError)/dt;             //D term
+//    if (currentError < 0.5f) {
+//        printf("Reached final position: %f\n\r", currentRevs);
+//        commandFinished = true;
+//        return;
+//    }
+    dError = (currentError - prevError)/dtControl;             //D term
     duty = k*(kp*currentError + kd*dError);             //Control motor duty
-    if (duty > 0) lead = -2;
-    else {                                  //if duty < 0, reverse motor spin           
+    if (duty > 0) {
+        lead = -2;
+    } else {                                  //if duty < 0, reverse motor spin           
         lead = 2;                           // direction to decelerate
         duty = -duty;
     }
 }
 
+void rotate(float r) {
+    printf("Rotating for %f revolutions\n\r", r);
+    goalRevs = r;
+    state = updateState();
+    // Kickstart
+    motorOut((state-orState+lead+6)%6, 0.3f);
+    controlTicker.attach(&controlPosition, dtControl);
+    motorOutTicker.attach(&runMotor, dtMotor);
+}
+
 void i1rise(){
     state = updateState();
-    motorOut((state-orState+lead+6)%6, duty);
+    //motorOut((state-orState+lead+6)%6, duty);
     
     if (I3.read() == 1) {                   //Only count revolutions if the
         count_i3++;                         // rotor spins forward
@@ -88,7 +124,7 @@
 //TODO merge with i_edge by measuring angular velocity in i1rise.
 void i3rise(){
     state = updateState();
-    motorOut((state-orState+lead+6)%6, duty);
+//    motorOut((state-orState+lead+6)%6, duty);
     
     w3 = angle/dt_I3.read();                //Calc angular velocity
     
@@ -97,7 +133,7 @@
 
 void i_edge(){                              //Upon status led interrupt, update
    state = updateState();                   // the motor output
-   motorOut((state-orState+lead+6)%6, duty);
+//   motorOut((state-orState+lead+6)%6, duty);
 }
 //Todo: add comments on this fucntion
 void updateDiskPosition() {
@@ -160,7 +196,6 @@
     
     dt_I3.start();                      //Start the time counters for velocity
 
-    controlTicker.attach(&control, dt); //Adjust position every 2ms
     
     I1.rise(&i1rise);                   //Assign interrupt handlers for LEDs
     I1.fall(&i_edge);
@@ -174,22 +209,31 @@
     CHB.rise(&CHB_rise);
     CHB.fall(&CHB_fall);
 
-    state = updateState();              //Kickstart the motor
-    motorTimer.start();
-    motorOut((state-orState+lead+6)%6, 0.3f);
-    
-    while (count_i3 <= goalRevs+1) {
-        pc.printf("Speed: %f, duty cycle: %f, revs done: %d, dError: %f , currentError: %f, prevError: %f, currentRevs: %f \n\r",w3, duty, count_i3, dError, currentError, prevError, currentRevs);
+//    state = updateState();
+//    // Kickstart
+//    motorOut((state-orState+lead+6)%6, 0.3f);
+//    controlTicker.attach(&controlPosition, dtControl);
+//    motorOutTicker.attach(&runMotor, dtMotor);
+    printf("Ready\n\r");
+    while (true) {
+      if (count_i3 >= goalRevs && !commandFinished) commandFinished = true;
+//        pc.printf("Speed: %f, duty cycle: %f, revs done: %d, dError: %f , currentError: %f, prevError: %f, currentRevs: %f \n\r",w3, duty, count_i3, dError, currentError, prevError, currentRevs);
+//        printf("dError: %f , currentError: %f, prevError: %f, currentRevs: %f\n\r",dError, currentError, prevError, currentRevs);
     // Work with user input here
       if (commandReady) {
-        printf("Got command: %d\n\r", parseResult.mode);
+//        printf("Got command: %d\n\r", parseResult.mode);
         commandReady = false;
         readyForCommand = false;
+        commandFinished = false;
         runCommand(parseResult);
+      }
+      if (commandFinished && !readyForCommand) {
+        motorOutTicker.detach();
+        stopMotor();                        //Turn off the motor if position is reached
         readyForCommand = true;
+        printf("Ready\n\r");
       }
     }
-    stopMotor();                        //Stop the motor if position is reached
     return 0;
 }