Diff: main.cpp

Revision:

0:53f83cb0b38d

Child:

1:718c43124021

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 16 10:15:55 2015 +0000
@@ -0,0 +1,93 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "biquadFilter.h"
+
+/*programma voor het testen van de beweging van de arm, aangestuurd door twee potmeters*/
+
+//INPUT/OUTPUT
+PwmOut pwm_motor1(D5); //onderste motor
+DigitalOut dir_motor1(D4); 
+PwmOut pwm_motor2(D6); //bovenste motor
+DigitalOut dir_motor2(D7);
+Encoder encoder1(D13,D12,true);
+Encoder encoder2(D10,D9,true);
+//potmeters voor testen:
+AnalogIn potmeter1(A4);
+AnalogIn potmeter2(A5);
+
+//VARIABELEN
+Ticker control_ticker;
+double pos_1;
+double pos_2;
+
+// Sample time (motor1−timestep)
+const double m1_Ts = 0.01;
+// Controller gains (motor1−Kp,−Ki,...)
+const double m1_Kp = 5, m1_Ki = 0.005, m1_Kd = 2;
+double m1_err_int = 0, m1_prev_err = 0;
+// Derivative filter coeicients (motor1−filter−b0,−b1,...)
+const double m1_f_a1 = -0.11175688680, m1_f_a2 = 0.17749674417, m1_f_b0 = 1.0, m1_f_b1 = 2, m1_f_b2 = 1;
+
+
+// Sample time (motor1−timestep)
+const double m2_Ts = 0.01;
+// Controller gains (motor1−Kp,−Ki,...)
+const double m2_Kp = 5, m2_Ki = 0.005, m2_Kd = 2;
+double m2_err_int = 0, m2_prev_err = 0;
+// 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;
+
+
+// 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)
+{
+// 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);
+    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()-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 );
+    if(pos_1 > 0)
+        dir_motor1 = 0;
+    else
+        dir_motor1 = 1;
+
+    pwm_motor1.write(abs(pos_1));
+
+} // Attach this function to a Ticker
+
+void m2_Controller()
+{
+    double reference = (potmeter1.read()-0.5)*4200;
+    double position = encoder1.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 );
+    if(pos_2 > 0)
+        dir_motor2 = 0;
+    else
+        dir_motor2 = 1;
+
+    pwm_motor2.write(abs(pos_2));
+
+} // Attach this function to a Ticker
+
+int main()
+{
+    pwm_motor1.period(0.0001);
+    pwm_motor2.period(0.0001);
+    control_ticker.attach(&m1_Controller, 0.01);
+    control_ticker.attach(&m2_Controller, 0.01);
+
+    while(1) {}  
+}
\ No newline at end of file