Diff: main.cpp

Revision:

20:c3bdb8f73c02

Parent:

18:12598db37e38

Child:

21:dd4bbb617415

--- a/main.cpp	Tue Mar 14 22:27:58 2017 +0000
+++ b/main.cpp	Tue Mar 14 23:40:16 2017 +0000
@@ -3,10 +3,11 @@
 #include "definitions.h"
 #include "motorControl.h"
 #include "parser.h"
+#include <cmath>
 
-#define kp 0.75f
-#define ki 0.5f
-#define kd 1.0f
+#define kp 0.3f
+#define ki 0.05f
+#define kd 0.5f
 #define dt 0.02f                        //given in ms, used to call the PID c.
 
 volatile uint8_t state = 0;
@@ -29,7 +30,7 @@
 Ticker controlTicker;
 
 volatile float fi0 = 0;                 //number of revs done
-volatile int goalRevs = 50;
+volatile int goalRevs = 200;
 volatile float fi = 2*3.1415*goalRevs;
 volatile float goalW = 0;               //desired angular velocity
 volatile float accError = 0;
@@ -53,10 +54,12 @@
     fi0 = 6.283 * count_i3;             //fi0 = 2*pi*revs
     float error = fi - fi0;
     accError += error*dt;
-    float dError = (prevError - error)/dt;
+    float dError = (error - prevError)/dt;
     goalW = kp*error + ki*accError + kd*dError;
     prevError = error;
-    duty = getDuty(goalW);
+    duty = getDuty(abs(goalW));
+    if (goalW > 0) lead = -2;
+    else lead = 2;
 }
 
 void i3rise(){
@@ -107,7 +110,7 @@
     
     while (count_i3 <= goalRevs) {
 
-        pc.printf("Speed: %f, duty cycle: %f, revs done: %d \n\r",w3, duty, count_i3);
+        pc.printf("Speed: %f, goal speed: %f, duty cycle: %f, revs done: %d \n\r",w3, goalW, duty, count_i3);
         /*
         if(duty < 0.00f) {
             stopMotor();
@@ -120,6 +123,8 @@
         }
         */
     }
-    stopMotor();
+    while (abs(w3) > 50){
+        pc.printf("Speed: %f, goal speed: %f, duty cycle: %f, revs done: %d \n\r",w3, goalW, duty, count_i3);
+    }
     return 0;
 }
\ No newline at end of file