Diff: main.cpp

Revision:

1:718c43124021

Parent:

0:53f83cb0b38d

Child:

2:3835d7ff7600

diff -r 53f83cb0b38d -r 718c43124021 main.cpp
--- a/main.cpp	Fri Oct 16 10:15:55 2015 +0000
+++ b/main.cpp	Mon Oct 19 13:00:26 2015 +0000
@@ -19,6 +19,7 @@
 Ticker control_ticker;
 double pos_1;
 double pos_2;
+double m1_f_v1 = 0, m1_f_v2 = 0;
 
 // Sample time (motor1−timestep)
 const double m1_Ts = 0.01;
@@ -37,14 +38,24 @@
 // Derivative filter coeicients (motor1−filter−b0,−b1,...)
 const double m2_f_a1 = -0.11175688680, m2_f_a2 = 0.17749674417, m2_f_b0 = 1.0, m2_f_b1 = 2, m2_f_b2 = 1;
 
+double biquad( double u, double &v1, double &v2, const double a1, const double a2,
+               const double b0, const double b1, const double b2 )
+{
+    double v = u - a1*v1 - a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
 
 // Reusable PID controller with filter
-double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2)
+double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2)
 {
 // Derivative
     double e_der = (e - e_prev)/Ts;
-    biquadFilter derfilter(f_a1, f_a2, f_b0, f_b1, f_b2);
-    e_der = derfilter.step(e_der);
+    /*biquadFilter derfilter(f_a1, f_a2, f_b0, f_b1, f_b2);
+    e_der = derfilter.step(e_der);*/
+    e_der = biquad( e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2 );
     e_prev = e;
 // Integral
     e_int = e_int + Ts * e;
@@ -57,7 +68,7 @@
     double reference = (potmeter1.read()-0.5)*4200;
     double position = encoder1.getPosition(); // Don’t use magic numbers!
     double error = (reference - position)/1000;
-    pos_1 = PID( error, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2 );
+    pos_1 = PID( error, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2 );
     if(pos_1 > 0)
         dir_motor1 = 0;
     else
@@ -69,14 +80,14 @@
 
 void m2_Controller()
 {
-    double reference = (potmeter1.read()-0.5)*4200;
-    double position = encoder1.getPosition(); // Don’t use magic numbers!
+    double reference = (potmeter2.read()-0.5)*4200;
+    double position = encoder2.getPosition(); // Don’t use magic numbers!
     double error = (reference - position)/1000;
-    pos_2 = PID( error, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2 );
+    pos_2 = PID( error, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m1_f_v1, m1_f_v2, m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2 );
     if(pos_2 > 0)
+        dir_motor2 = 1;
+    else
         dir_motor2 = 0;
-    else
-        dir_motor2 = 1;
 
     pwm_motor2.write(abs(pos_2));