Diff: controlandadjust.cpp

Revision:

18:ef413d2fd0b1

Parent:

17:666505754e3f

Child:

19:e3585d3c5a85

--- a/controlandadjust.cpp	Mon Oct 26 12:25:31 2015 +0000
+++ b/controlandadjust.cpp	Mon Oct 26 12:37:56 2015 +0000
@@ -161,8 +161,8 @@
     error1_int = error1_int + Ts * error1;
     error2_int = error2_int + Ts * error2;
 // PID
-     signaal1= signaal1*(tau_p/Ts) + Kp * error1 + Ki * error1_int + Kd * error1_der;
-     signaal2= signaal2*(tau_p/Ts) + Kp * error2 + Ki * error2_int + Kd * error2_der;
+     signaal1= (signaal1*(tau_p/Ts) + Kp * error1 + Ki * error1_int + Kd * error1_der)*( 1/( 1+ (tau_p/Ts) ) );
+     signaal2= (signaal2*(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"