Revision 20:a9a6ffb32dbf, committed 2015-10-29

Comitter:

Gerth

Date:

Thu Oct 29 14:50:14 2015 +0000

Parent:

19:e3585d3c5a85

Commit message:

added correcttion for the angles which are introduces bij "bevestigingslip"

Changed in this revision

--- a/controlandadjust.cpp	Thu Oct 29 14:02:20 2015 +0000
+++ b/controlandadjust.cpp	Thu Oct 29 14:50:14 2015 +0000
@@ -147,38 +147,7 @@
         error1_int=0;
     }
 }
-//////////////////////////////////////PID WITH LOW PASS FILTER
-void controlandadjust::PIDLowPass(double error1, double error2, float Kp, float Ki,float Kd, double tau_p)
-{
-    // Derivative
-    double error1_der = (error1 - error1_prev)/Ts; //(e(k)-e(k-1))/Ts
-    error1_prev = error1;
 
-    double error2_der = (error2 - error2_prev)/Ts;
-    error2_prev = error2;
-
-// Integral
-    error1_int = error1_int + Ts * error1;
-    error2_int = error2_int + Ts * error2;
-// PID
-     signal1= (signal1*(tau_p/Ts) + Kp * error1 + Ki * error1_int + Kd * error1_der)*( 1/( 1+ (tau_p/Ts) ) );
-     signal2= (signal2*(tau_p/Ts) + Kp * error2 + Ki * error2_int + Kd * error2_der)*( 1/( 1+ (tau_p/Ts) ) );
-
-    //check if error is big enough to produce signal, else signal is 0 to save motors
-    //and make error_int 0 if the error is small enough to protect from "spooling up"
-    if (fabs(error1) > minimal_error && fabs(error2) > minimal_error) {
-        process_signal(signal1,signal2);
-    } else if (fabs(error1) <= minimal_error && fabs(error2) <= minimal_error) {
-        process_signal(0,0);
-        error1_int=0,error2_int=0;
-    } else if (fabs(error1) > minimal_error && fabs(error2) <= minimal_error) {
-        process_signal(signal1,0);
-        error2_int=0;
-    } else if (fabs(error1) <= minimal_error && fabs(error2) > minimal_error) {
-        process_signal(0,signal2);
-        error1_int=0;
-    }
-}
 
 void controlandadjust::STOP()
 {

--- a/controlandadjust.h	Thu Oct 29 14:02:20 2015 +0000
+++ b/controlandadjust.h	Thu Oct 29 14:50:14 2015 +0000
@@ -59,17 +59,6 @@
     */
     void PID(double error1, double error2, float Kp, float Ki, float Kd );
              
-             /** PID controller with low pass
-    * @param error1 : double Error from motor 1
-    * @param error2 : double Error from motor 2
-    * @param Kp : float Desired value of Kp for your controller
-    * @param Ki : float Desired value of Ki for your controller
-    * @param Kd : float Desired value of Kd for your controller
-    * @param tau_p : double Value of tau p (for the low pass filter)
-    * @return void
-    */
-    void PIDLowPass(double error1, double error2, float Kp, float Ki,float Kd,double tau_p);
-
     /** Read the PWM signal to motor 1
     * @return PWM signal to motor 1
     */