Diff: main.cpp

Revision:

1:49f60db70b5a

Parent:

0:28db0afc950f

Child:

2:707567853e34

--- a/main.cpp	Thu Sep 24 16:04:43 2015 +0000
+++ b/main.cpp	Thu Sep 24 16:24:59 2015 +0000
@@ -23,12 +23,10 @@
 //const float pwm_frequency=1000;
 const double hidscope_frequency=100;
 const double pid_control_frequency=5;
-const double filter_pot1_frequency=200;
 
 //tickers
 Ticker hidscope_ticker;
 Ticker pid_control_ticker;
-Ticker filter_pot1_ticker;
 
 //constants
 const int cpr=32*131;
@@ -37,7 +35,7 @@
 
 //////////////////////////////////////////////////////////FILTERS///////////////////////////////////////////////////////
 
-//another more powerful low pass filter
+//IK WEET NIET HOE GOED DEZE WAARDES KLOPPEN, VOORZICHTIG DUS!! WAARDES ZIJN TE VERKRIJGEN UIT ASN
 //Biquad #1
 const double gain_f1 = 0.004756;
 const double B_f1[] = { 1.00000000000, -0.34966580719,  1.00000000000};
@@ -66,18 +64,33 @@
     return y;
 }
 ///////////////////////////////////////////////////////////CONTROLLERS//////////////////////////////////////////////////////////
+//DIT ZIJN ZOMAAR WAARDEN, AANPASSEN DUS WAARDES BEREKENEN MET MATLAB
 const float motor1_pid_kp=0.5;
+const float motor1_pid_ki=0.01;
+const float motor1_pid_kd=0.01;
+
+double motor1_error_int=0;
+double motor1_error_prev=0;
 
-//re usable P controller
-double p_control(float error,float kp)
+// Reusable PID controller with filter
+double pid_control( double e, const double Kp, const double Ki, const double Kd, double Ts,//Ts is dus sampletijd
+                    double &motor1_error_int, double &motor1_error_prev)
 {
-    return (error*kp);
+// Derivative
+    double e_der = (e - motor1_error_prev)/Ts;
+    e_der = biquad( e_der, v1, v2, a1_f1, a2_f1, b0_f1, b1_f1, b2_f1);//twee keer hetzelfde met andere filterwaardes, dit klopt
+    e_der = biquad( e_der, v1, v2, a1_f2, a2_f2, b0_f2, b1_f2, b2_f2);//is omdat een biquad filter maar 6 polen en nulpunten heeft
+    motor1_error_prev = e;
+// Integral
+    motor1_error_int = motor1_error_int + Ts * e;
+// PID
+    return Kp * e + Ki * motor1_error_int + Kd * e_der;
 }
 
 
 ///////////////////////////////////////////////////////////GO-FLAGS AND ACTIVATION FUNCTIONS///////////////////////////////
 //go flags
-volatile bool scopedata_go=false, pid_control_go=false, filter_pot1_go=false;
+volatile bool scopedata_go=false, pid_control_go=false; 
 
 //acvitator functions
 
@@ -89,10 +102,6 @@
 {
     pid_control_go=true;
 }
-void filter_pot1_activate()
-{
-    filter_pot1_go=true;
-}
 ///////////////////////////////////////////////////////FUNCTIONS//////////////////////////////////////////////////////////////////////////
 
 //scopedata
@@ -108,32 +117,21 @@
 int main()
 {
     //set initial shizzle
-    //motor1_speed_control.period(1.0/pwm_frequency);
     motor1_speed_control.write(0);
 
     //tickers
     hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
-    pid_control_ticker.attach(&pid_control_activate,1.0/p_control_frequency);
-    filter_pot1_ticker.attach(&filter_pot1_activate,1.0/filter_pot1_frequency);
+    pid_control_ticker.attach(&pid_control_activate,1.0/pid_control_frequency);
 
     //make variables
     volatile float signal_motor1=0;
-    volatile double pot1_filtered=0;
     while(1) {
-        
-        //filter signal with low pass biquad filter
-        if (filter_pot1_go==true) {
-            double pass1, pass2;
-            pass1=biquad(pot1.read(),v1,v2,a1_f1, a2_f1, b0_f1, b1_f1, b2_f1);
-            pass2=biquad(pass1,v1,v2,a1_f2, a2_f2, b0_f2, b1_f2, b2_f2);
-            pot1_filtered=pass2;
-            filter_pot1_go=false;
-        }
 
-        //control motor 1 with a p controller
-        if (p_control_go==true) {
-            double error=2*PI*pot1_filtered-counttorad*encoder1.getPulses();
-            double signal_motor1=pid_control(error,motor1_pid_kp);//send error to pid controller
+        //control motor 1 with a pID controller
+        if (pid_control_go==true) {
+            double error=2*PI*pot1.read()-counttorad*encoder1.getPulses();
+            double signal_motor1=pid_control(error,motor1_pid_kp,motor1_pid_ki,motor1_pid_kd,
+                                             1.0/pid_control_frequency,motor1_error_int,motor1_error_prev);//send error to pid controller 1.0/pid_control_frequency=sampletijd
             if (signal_motor1>=0) {//determine CW or CCW rotation
                 motor1_direction.write(CW);
             } else {
@@ -146,7 +144,7 @@
                 signal_motor1=fabs(signal_motor1);// if signal<1 use signal
             }
             motor1_speed_control.write(fabs(signal_motor1));//write signal to motor
-            p_control_go=false;
+            pid_control_go=false;
         }
         //call scopedata
         if (scopedata_go==true) {