Diff: main.cpp

Revision:

0:26ce65a63616

Child:

1:e5d2d638eaf4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 14 09:26:14 2016 +0000
@@ -0,0 +1,96 @@
+#include "stdio.h"
+#include "math.h"
+#include "mbed.h"
+#include "QEI.h"
+#include "MODSERIAL.h"
+
+MODSERIAL pc(USBTX, USBRX);
+QEI wheel_M1 (D10, D11, NC, 64);
+QEI wheel_M2 (D13, D12, NC, 64);
+PwmOut pwm_M1 (D5);
+PwmOut pwm_M2 (D6);
+DigitalOut dir_M1 (D4);
+DigitalOut dir_M2 (D7);
+
+Ticker PIDcontrol;
+
+volatile double q1;
+volatile double q2;
+volatile double q1_prev = 0;
+volatile double q2_prev = 0;
+volatile double pwm_M1_ref = 0;
+volatile double pwm_M2_ref = 0;
+volatile double Pulses_M1;
+volatile double Pulses_M2;
+
+void initialize()
+{
+    dir_M1 = 0; //ccw
+    dir_M2 = 1; //cw
+    while (q1 > -20*2*3.1415/360) {
+        Pulses_M1 = wheel_M1.getPulses();    //1334.355 counts/rad
+        q1 = Pulses_M1 / (1334.355/3);     // rad
+        pwm_M1_ref = 0.1;
+    }
+    pwm_M1_ref = 0;
+
+    //while (q2 < 45*2*3.1415/360) {
+    //    Pulses_M2 = wheel_M2.getPulses();
+    //    q2 = Pulses_M2 / (1334.355/3);
+    //    pwm_M2_ref = 0.1;
+    //}
+    //pwm_M2_ref = 0;
+
+const double M1_TS = 0.002;
+const double M1_Kp = 4.348, M1_Ki = 36.632, M1_Kd = 0.126;
+double M1_err_int = 0, n1_prev_err = 0;
+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; 
+double M2_f_v2 = 0, m2_f_v2 = 0;
+
+//Biquad filter
+
+//volatile double u;
+//volatile double v;
+//volatile double v1;
+//volatile double v2;
+//volatile double y;
+//double a1;
+//double a2;
+//double b0;
+//double b1;
+//double b2;
+
+double biquad(double u, double &v1, double &v2, 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;
+}
+
+//PID 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_b2){
+    // 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 = pwm_M1;
+    q1 = wheel_M1.getPulses() / (1334.355/3);
+    double velocity = (q1 - q1_prev) / 0.002;
+    q1_prev = q1;
+    pwm_M1 = PID(reference - velocity, 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);
+}
+
+int main()
+{
+    pc.baud(115200);
+    PIDcontrol.attach(&M1_controller, 0.002f);
+    initialize();
+}
\ No newline at end of file