Diff: controlandadjust.cpp

Revision:

16:57c4f4ac11e4

Parent:

15:a1a29db96f4f

Child:

17:666505754e3f

--- a/controlandadjust.cpp	Tue Oct 20 15:14:14 2015 +0000
+++ b/controlandadjust.cpp	Fri Oct 23 12:13:00 2015 +0000
@@ -13,6 +13,10 @@
 const float pi=3.14159265359;
 const float degtorad=(pi/180);
 
+
+//
+Serial pcshit(USBTX,USBRX);
+
 DigitalOut motor1_dir(D7);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
 PwmOut motor1_speed(D6);//aanstuursnelheid motor 1
 PwmOut motor2_speed(D5);
@@ -85,7 +89,6 @@
     } else if (fabs(error1) <= minimal_error && fabs(error2) > minimal_error) {
         verwerksignaal(0,signaal2);
     }
-
 }
 
 void controlandadjust::PI(float error1, float error2, float Kp, float Ki,float Ts, float &error1_int, float &error2_int)
@@ -97,14 +100,18 @@
     float signaal2=Kp*error2+Ki*error2_int;
 
      //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) {
         verwerksignaal(signaal1,signaal2);
     } else if (fabs(error1) <= minimal_error && fabs(error2) <= minimal_error) {
         verwerksignaal(0,0);
+        error1_int=0,error2_int=0;
     } else if (fabs(error1) > minimal_error && fabs(error2) <= minimal_error) {
         verwerksignaal(signaal1,0);
+        error2_int=0;
     } else if (fabs(error1) <= minimal_error && fabs(error2) > minimal_error) {
         verwerksignaal(0,signaal2);
+        error1_int=0;
     }
 }
 
@@ -126,14 +133,18 @@
     float signaal2= Kp * error2 + Ki * error2_int + Kd * error2_der;
 
      //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) {
         verwerksignaal(signaal1,signaal2);
     } else if (fabs(error1) <= minimal_error && fabs(error2) <= minimal_error) {
         verwerksignaal(0,0);
+        error1_int=0,error2_int=0;
     } else if (fabs(error1) > minimal_error && fabs(error2) <= minimal_error) {
         verwerksignaal(signaal1,0);
+        error2_int=0;
     } else if (fabs(error1) <= minimal_error && fabs(error2) > minimal_error) {
         verwerksignaal(0,signaal2);
+        error1_int=0;
     }
 }