Diff: main.cpp

Revision:

1:64ec36ba6e76

Parent:

0:88e83d97a0ee

Child:

2:323606a75d64

--- a/main.cpp	Mon Oct 12 13:58:40 2015 +0000
+++ b/main.cpp	Tue Oct 13 13:26:12 2015 +0000
@@ -1,9 +1,12 @@
-#include 'mbed.h'
+#include "mbed.h"
+#include "encoder.h"
 
 Ticker control_ticker;
 PwmOut pwm_motor1(D4); //onderste motor
 DigitalOut dir_motor1(D5);
 AnalogIn potmeter1(A4);
+Encoder encoder1(D13,D12,true);
+double pos_1;
 
 // Sample time (motor1−timestep)
 const double m1_Ts = 0.01;
@@ -19,40 +22,37 @@
 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;
+    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)
+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;
+    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;
+    e_int = e_int + Ts * e;
 // PID
-    return Kp ∗ e + Ki ∗ e_int + Kd ∗ e_der;
+    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)
+    double position = 0.002991*encoder1.getPosition(); // Don’t use magic numbers!
+    pos_1 = 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(pos_1 > 0)
         dir_motor1 = 0;
     else
         dir_motor1 = 1;
 
-    pwm_motor1.write(abs(motor1));
+    pwm_motor1.write(abs(pos_1));
 } // Attach this function to a Ticker
 
 int main()