Diff: main.cpp

Revision:

0:88e83d97a0ee

Child:

1:64ec36ba6e76

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 12 13:58:40 2015 +0000
@@ -0,0 +1,63 @@
+#include 'mbed.h'
+
+Ticker control_ticker;
+PwmOut pwm_motor1(D4); //onderste motor
+DigitalOut dir_motor1(D5);
+AnalogIn potmeter1(A4);
+
+// Sample time (motor1−timestep)
+const double m1_Ts = 0.01;
+// Controller gains (motor1−Kp,−Ki,...)
+const double m1_Kp = 2.5, m1_Ki = 1.0, m1_Kd = 0.5;
+double m1_err_int = 0, m1_prev_err = 0;
+// Derivative filter coeicients (motor1−filter−b0,−b1,...)
+const double m1_f_a1 = 1.0, m1_f_a2 = 2.0, m1_f_b0 = 1.0, m1_f_b1 = 3.0, m1_f_b2 = 4.0;
+// Filter variables (motor1−filter−v1,−v2)
+double m1_f_v1 = 0, m1_f_v2 = 0;
+
+// Biquad filter (See slide 14)
+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, 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_b3)
+{
+// Derivative
+    double e_der = (e − e_prev)/Ts;
+    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;
+// PID
+    return Kp ∗ e + Ki ∗ e_int + Kd ∗ e_der;
+}
+
+void m1_Controller()
+{
+    double reference = potmeter1.read();
+    double position = 0.002991∗encoder1.getPosition(); // Don’t use magic numbers!
+    motor1 = PID( reference − position, 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(motor1 > 0)
+        dir_motor1 = 0;
+    else
+        dir_motor1 = 1;
+
+    pwm_motor1.write(abs(motor1));
+} // Attach this function to a Ticker
+
+int main()
+{
+    control_ticker.attach(m1_Controller, 0.01);
+
+    while(1);
+}
\ No newline at end of file