Diff: main.cpp

Revision:

38:e57ffbd9b75d

Parent:

37:bf96972df861

Child:

39:ae41a212c170

--- a/main.cpp	Fri Mar 17 19:24:08 2017 +0000
+++ b/main.cpp	Fri Mar 17 20:27:50 2017 +0000
@@ -20,27 +20,28 @@
 Timer motorTimer;
 Ticker controlTicker;
 
-volatile float fi0 = 0;                 //number of revs done
-volatile int goalRevs = 50;
-volatile float fi = 2*3.1415*goalRevs;
-volatile float accError = 0;
-volatile float prevError = 0;
-float dError = 0;
+volatile float currentRevs = 0.0;                 //number of revs done
+volatile float goalRevs = 999.0;
+volatile float prevError = 0.0;
+volatile double dError = 0.0;
+volatile float currentError = 0.0;
 
-#define kp 0.001f
+#define kp 0.009f
 #define ki 0.0f //0.05f
-#define kd 0.005f //0.5f
-#define k 1.0f
+#define kd 0.025f //0.5f
+#define k 10.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.283f / diskPosition + 6.283f*count_i3;    // 2pi/360degr + 2pi*revs
-    float error = fi - fi0;
-    accError += error*dt;
-    dError = (error - prevError)/dt;
-    duty = k*(kp*error + ki*accError + kd*dError);
-    prevError = error;
+    if (w3 > 300) {
+        lead = 2;
+        return;
+    }
+    prevError = currentError;
+    currentRevs = diskPosition / 360 + count_i3;    // 1/360degr + 2pi*revs
+    currentError = goalRevs - currentRevs;
+    dError = (currentError - prevError)/dt;
+    duty = k*(kp*currentError + kd*dError);
     if (duty > 0) lead = -2;
     else {
         lead = 2;
@@ -136,16 +137,11 @@
     state = updateState();
     motorTimer.start();
     motorOut((state-orState+lead+6)%6, 0.3f);            //Kickstart the motor
-
-    while (count_i3 <= goalRevs) {
+    while (count_i3 <= goalRevs+1) {
         //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, dError: %f , disk position: %f \n\r",w3, duty, count_i3, dError, fi0);
+        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);
         //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();
-    while(1) {}
+    stopMotor();
     return 0;
 }
\ No newline at end of file