Diff: main.cpp

Revision:

25:0ee6b164f234

Parent:

24:487c898c8d71

Child:

31:bfb4629ca327

--- a/main.cpp	Wed Mar 15 18:14:40 2017 +0000
+++ b/main.cpp	Wed Mar 15 21:24:42 2017 +0000
@@ -5,9 +5,9 @@
 #include "parser.h"
 #include <cmath>
 
-#define kp 0.1f
+#define kp 0.026f
 #define ki 0.0f //0.05f
-#define kd 0.20f //0.5f
+#define kd 0.50f //0.5f
 #define dt 0.002f                        //given in ms, used to call the PID c.
 
 volatile uint8_t state = 0;
@@ -31,7 +31,7 @@
 Ticker controlTicker;
 
 volatile float fi0 = 0;                 //number of revs done
-volatile int goalRevs = 200;
+volatile int goalRevs = 50;
 volatile float fi = 2*3.1415*goalRevs;
 volatile float goalW = 0;               //desired angular velocity
 volatile float accError = 0;
@@ -58,9 +58,13 @@
     float dError = (error - prevError)/dt;
     goalW = kp*error + ki*accError + kd*dError;
     prevError = error;
-    duty = getDuty(abs(goalW));
+    if (abs(goalW) > w3) duty = 1 - w3/abs(goalW);
+    else duty = abs(goalW) / w3;
+    //duty = getDuty(abs(goalW));
     if (goalW > 0) lead = -2;
     else lead = 2;
+    //if (duty > 0) lead = -2;
+    //else lead = 2;
 }
 
 void i3rise(){
@@ -70,7 +74,7 @@
     w3 = angle/dt_I3.read();            //Calc angular velocity
     
     dt_I3.reset();
-    count_i3++;                 
+    count_i3++;
 }
 
 void i_edge(){
@@ -108,7 +112,7 @@
 
     state = updateState();
     motorTimer.start();
-    motorOut((state-orState+lead+6)%6, 1.0f);            //Kickstart the motor
+    motorOut((state-orState+lead+6)%6, 0.3f);            //Kickstart the motor
     /*
     while (motorTimer.read() < 30) {
         motorOut((state-orState+1+6)%6, 0.75f);
@@ -133,7 +137,7 @@
         }
         */
     }
-    while (abs(w3) > 40){
+    while (abs(w3) > 10){
         pc.printf("Speed: %f, goal speed: %f, duty cycle: %f, revs done: %d \n\r",w3, goalW, duty, count_i3);
     }
     stopMotor();