Diff: main.cpp

Revision:

36:747d80e3aca9

Parent:

35:5857105c9956

Child:

37:bf96972df861

--- a/main.cpp	Thu Mar 16 21:49:07 2017 +0000
+++ b/main.cpp	Fri Mar 17 02:13:10 2017 +0000
@@ -9,6 +9,13 @@
 volatile float duty = 0.5;
 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
+
 Timer dt_I3;
 Timer motorTimer;
 Ticker controlTicker;
@@ -19,25 +26,22 @@
 volatile float accError = 0;
 volatile float prevError = 0;
 
-#define kp 0.0016f
+#define kp 0.000683f
 #define ki 0.0f //0.05f
-#define kd 0.00006f //0.5f
+#define kd 0.005f //0.5f
+#define k 1.0f
 #define dt 0.002f                        //given in ms, used to call the PID c.
 
 void control(){
-    fi0 = 6.283 * count_i3;             //fi0 = 2*pi*revs
+    //fi0 = 6.283 * count_i3;             //fi0 = 2*pi*revs
+    fi0 = 6.283f / diskPosition + 6.283f*count_i3;    // 2pi/360degr + 2pi*revs
     float error = fi - fi0;
     accError += error*dt;
     float dError = (error - prevError)/dt;
-    duty = kp*error + ki*accError + kd*dError;
+    duty = k*(kp*error + ki*accError + kd*dError);
     prevError = error;
-    //if (abs(goalW) > w3) duty = 1 - w3/abs(goalW);
-    //else duty = abs(goalW) / w3;
-    //duty = getDuty(abs(goalW));
     if (duty > 0) lead = -2;
     else lead = 2;
-    //if (duty > 0) lead = -2;
-    //else lead = 2;
 }
 
 void i3rise(){
@@ -53,15 +57,57 @@
 void i_edge(){
    state = updateState();
    motorOut((state-orState+lead+6)%6, duty);
-}  
-
-void CHA_rise(){
 }
-void CHA_fall(){
+
+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 CHB_rise(){
+
+void updateRelativeState() {
+  CH_state = relativeStateMap[CHB_state + 2*CHA_state];
+}
+
+void CHA_rise() {
+  CHA_state = 1;
+  updateRelativeState();
+  updateDiskPosition();
 }
-void CHB_fall(){
+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() {
@@ -78,21 +124,24 @@
     I3.rise(&i3rise);
     I3.fall(&i_edge);
     
-//    CHA.rise(&CHA_rise);
-//    CHA.fall(&CHA_fall);
-//    CHB.rise(&CHB_rise);
-//    CHB.fall(&CHB_fall);
+    CHA.rise(&CHA_rise);
+    CHA.fall(&CHA_fall);
+    CHB.rise(&CHB_rise);
+    CHB.fall(&CHB_fall);
 
     state = updateState();
     motorTimer.start();
     motorOut((state-orState+lead+6)%6, 0.3f);            //Kickstart the motor
 
     while (count_i3 <= goalRevs) {
-        pc.printf("Speed: %f, duty cycle: %f, revs done: %d \n\r",w3, duty, count_i3);
+        //pc.printf("Speed: %f, duty cycle: %f, revs done: %d \n\r",w3, duty, count_i3);
+        pc.printf("Speed: %f, duty cycle: %f, revs done: %d, disk position: %f \n\r",w3, duty, count_i3, fi0);
+        //pc.printf("Disk position: %f \n\r",fi0);
     }
     while (abs(w3) > 10){
         pc.printf("Speed: %f, duty cycle: %f, revs done: %d \n\r",w3, duty, count_i3);
     }
-    stopMotor();
+    //stopMotor();
+    while(1) {}
     return 0;
 }
\ No newline at end of file