Diff: main.cpp

Revision:

26:4f84448b4d46

Parent:

25:df780572cfc8

Child:

27:a4228ea8fb8f

--- a/main.cpp	Fri Oct 06 12:37:54 2017 +0000
+++ b/main.cpp	Fri Oct 06 13:11:24 2017 +0000
@@ -75,16 +75,20 @@
     pc.printf("%0.2f revolutions \r\n", motor1Position);
     e_pos = getReferencePosition() - motor1Position;
     e_int = e_int + Ts*e_pos;
-    e_der = e_der - e_prev;
-    e = e_pos + e_int;
+    e_der = (e_pos - e_prev)/Ts;
+    e = e_pos + e_int + e_der; // not sure if this is really even all that useful
+    e_prev = e_pos; // Store current error as we'll need it to compute the next derivative error
     pc.printf("Error is %0.2f \r \n", e);
     }
 
+const float k_p = 0.1; // use potmeter 2 to adjust k_p within range 0 to 1
+const float k_i = 0.05; // Still needs a reasonable value
+const float k_d = 0.01; // Again, still need to pick a reasonable value
+
 // motorController sets a motorValue based on the position error. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward
-float motorController(float e_pos, float e_int){
-    float k_p = 5*pot2.read(); // use potmeter 2 to adjust k_p within range 0 to 1
-    float k_i = 0.001; // How do I pick a reasonable value for k_i?
-    double motorValue = (e_pos*k_p)/maxAngle + 0.35 + k_i*e_int; // motorValue is the normalized voltage applied to motor magnitude pin between -1 and 1 added 0.35 as motorvalue of 0.35 does nothing
+float motorController(float e_pos, float e_int, float e_der){    
+    // NOTE: do I still need the maxangle bit here?
+    double motorValue = (e_pos*k_p) + 0.35 + k_i*e_int + k_d*e_der; // motorValue is the normalized voltage applied to motor magnitude pin between -1 and 1 added 0.35 as motorvalue of 0.35 does nothing
     return motorValue;
     }
     
@@ -120,13 +124,14 @@
     }
     
 void measureAndControl(){
-    getError(e_pos, e_int);
-    float motorValue = motorController(e_pos, e_int);
-    pc.printf("Position error is %.2f \r \n", e_pos);
-    pc.printf("Total error is %.2f \r \n", e);
-    pc.printf("Integral error is %2f", e_int);
-    //pc.printf("Motorvalue is %.2f \r \n", motorValue);
-    //pc.printf("Motor direction is %d \r \n", motor1_direction);
+    getError(e_pos, e_int, e_der);
+    float motorValue = motorController(e_pos, e_int, e_der);
+    pc.printf("Position action is %.2f \r \n", k_p*e_pos);
+    pc.printf("Derivative action is %.2f \r \n", k_d*e_der);
+    pc.printf("Integral action is %.2f", k_i*e_int);
+    pc.printf("Total action is %.2f", k_p*e_pos + k_d*e_der + k_i*e_int);
+    pc.printf("Motorvalue is %.2f \r \n", motorValue);
+    pc.printf("Actual error is %.2f \r \n", e_pos);
     setMotor1(motorValue);
     }