Diff: main.cpp

Revision:

18:6f71bb91b8bd

Parent:

17:9b667e6e1290

Child:

19:9417d2011e8b

--- a/main.cpp	Wed Sep 30 15:20:14 2015 +0000
+++ b/main.cpp	Thu Oct 01 15:46:34 2015 +0000
@@ -37,6 +37,7 @@
 //Derivative filter coeffs
 const double BiGain = 0.016955;
 const double m2_f_a1 = -0.96608908283*BiGain, m2_f_a2 = 0.0*BiGain, m2_f_b0 = 1.0*BiGain, m2_f_b1 = 1.0*BiGain, m2_f_b2 = 0.0*BiGain;
+
 // Filter variables
 double m2_f_v1 = 0, m2_f_v2 = 0;
 
@@ -64,11 +65,12 @@
     return Kp * e + Ki*e_int + Kd*e_der;
 }
 
-// Motor control
+// Motor2 control
 void motor2_Controller() 
 {
     reference = m2_ref; // Setpoint
-    position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs
+    double pulses = Encoder.getPulses();
+    position = pulses*360/(0.5*128*131); // Aantal Degs
     double P2 = PID( reference - position, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, 
     m2_f_b0, m2_f_b1, m2_f_b2);
     motor2speed = abs(P2); // Speed control
@@ -80,7 +82,7 @@
     {
         motor2direction = 1;
     }
-    pc.printf("position = %f aantal degs = %f \n",reference,position);
+    pc.printf("position = %f aantal degs = %f, pulses = %d\n",reference,position, pulses);
 }
 
 int main()