Diff: main.cpp

Revision:

1:757aadb68807

Parent:

0:6ffe287c9e4c

diff -r 6ffe287c9e4c -r 757aadb68807 main.cpp
--- a/main.cpp	Tue Sep 22 11:52:38 2015 +0000
+++ b/main.cpp	Tue Sep 22 13:38:06 2015 +0000
@@ -2,11 +2,8 @@
 #include "HIDScope.h"
 #include "encoder.h"
 
-//hidscope met gewenst aantal kanalen
-HIDScope scope(2);
-
 //analoog in van potmeter 1
-AnalogIn pot1(A0);
+AnalogIn potmeter1(A0);
 
 //signaal naar motor uit
 DigitalOut motor1_direction(D7);// draairichting motor 1 (1 is CW )
@@ -15,77 +12,61 @@
 //DigitalOut motor2_direction(D4);
 
 //encoders
-Encoder motor1_encoder(D13,D12);
+Encoder encoder1(D13,D12);
 //Encoder motor2_encoder(D11,D10);
 
 //tickers
 Ticker scopedataticker;
 Ticker adjust_positionticker;
 
-//frequenties
-const float motor_frequency_pwm = 1000; //1kHz PWM
-const float scopedatafrequency=50;// frequentie waarmee informatie naar de scope gestuurd wordt
-const float adjust_position_frequency=8; // frequentie waarmee de motorpositie aangepast wordt
 
-//constanten
-const float cpr=4200;
+
 
 
-//go flags
-bool scopedata_go=false;
-bool adjust_position_go=false;
+// Sample time (motor1−timestep)
+const double m1_Ts = 0.01;
 
-//activators
-void scopedata_activate()
-{
-    scopedata_go=true;
-}
-void adjust_position_activate()
-{
-    adjust_position_go=true;
+// 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 coefficients (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*2;
+    v2 = v1; v1 = v;
+    return y;
 }
 
-//scopefunctie
-void scopedata()
-{
-    scope.set(0,pot1.read());
-    scope.set(1,motor1_encoder.getPosition());
-    scope.send();
+// 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_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;
 }
 
-//adjust position
-void adjust_position()
-{
-    float wantedposition=cpr*(pot1.read());
-    int actualposition=(motor1_encoder.getPosition());
-    if (wantedposition<=actualposition) {
-        motor1_direction=0;
-        motor1_speed_control=0.5;
-    } else if (wantedposition>=actualposition) {
-        motor1_direction=1;
-        motor1_speed_control=0.5;
-    } else {
-        motor1_direction=1;
-        motor1_speed_control=0;
-    }
-}
+void m1_Controller() {
+    double reference = potmeter1.read();
+    double position = 0.002991*encoder1.getPosition(); // Don’t use magic numbers!
+    double 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 );
+} // Attach this function to a Ticker
+
+
 
 int main ()
 {
-    motor1_speed_control.period(1/motor_frequency_pwm);
 
-    scopedataticker.attach(&scopedata_activate,1/scopedatafrequency);
-    adjust_positionticker.attach(&adjust_position_activate,1/adjust_position_frequency);
 
-    while(1) {
-        if (scopedata_go==true) {
-            scopedata();
-            scopedata_go=false;
-        }
-        if (adjust_position_go==true) {
-            adjust_position();
-            adjust_position_go=false;
-
-        }
-    }
 }