Diff: main.cpp

Revision:

31:bfb4629ca327

Parent:

25:0ee6b164f234

Child:

34:599634375ba0

--- a/main.cpp	Wed Mar 15 23:12:03 2017 +0000
+++ b/main.cpp	Thu Mar 16 21:15:26 2017 +0000
@@ -5,11 +5,6 @@
 #include "parser.h"
 #include <cmath>
 
-#define kp 0.026f
-#define ki 0.0f //0.05f
-#define kd 0.50f //0.5f
-#define dt 0.002f                        //given in ms, used to call the PID c.
-
 volatile uint8_t state = 0;
 volatile float w3 = 0;                  //Angular velocity
 volatile float duty = 0.5;
@@ -51,17 +46,22 @@
     return dutyCycles[28];
 }
 
+#define kp 0.0016f
+#define ki 0.0f //0.05f
+#define kd 0.00006f //0.5f
+#define dt 0.002f                        //given in ms, used to call the PID c.
+
 void control(){
     fi0 = 6.283 * count_i3;             //fi0 = 2*pi*revs
     float error = fi - fi0;
     accError += error*dt;
     float dError = (error - prevError)/dt;
-    goalW = kp*error + ki*accError + kd*dError;
+    duty = kp*error + ki*accError + kd*dError;
     prevError = error;
-    if (abs(goalW) > w3) duty = 1 - w3/abs(goalW);
-    else duty = abs(goalW) / w3;
+    //if (abs(goalW) > w3) duty = 1 - w3/abs(goalW);
+    //else duty = abs(goalW) / w3;
     //duty = getDuty(abs(goalW));
-    if (goalW > 0) lead = -2;
+    if (duty > 0) lead = -2;
     else lead = 2;
     //if (duty > 0) lead = -2;
     //else lead = 2;
@@ -75,11 +75,13 @@
     
     dt_I3.reset();
     count_i3++;
+    //PRINT("d")
 }
 
 void i_edge(){
    state = updateState();
    motorOut((state-orState+lead+6)%6, duty);
+   //PRINT("a")
 }  
 
 void CHA_rise(){