Diff: controlandadjust.cpp

Revision:

15:a1a29db96f4f

Parent:

14:2b4378bfe07a

Child:

16:57c4f4ac11e4

--- a/controlandadjust.cpp	Mon Oct 19 13:44:22 2015 +0000
+++ b/controlandadjust.cpp	Tue Oct 20 15:14:14 2015 +0000
@@ -9,6 +9,9 @@
 float direction2;
 float speed2;
 float maxvaluepwm=0;
+float minimal_error;
+const float pi=3.14159265359;
+const float degtorad=(pi/180);
 
 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
@@ -17,7 +20,10 @@
 
 
 
-controlandadjust::controlandadjust(void) {}
+controlandadjust::controlandadjust(float errorband)
+{
+    minimal_error=errorband*degtorad/2;
+}
 
 void controlandadjust::verwerksignaal(float signaal1,float signaal2)
 {
@@ -68,7 +74,18 @@
 {
     float signaal1=error1*Kp;
     float signaal2=error2*Kp;
-    verwerksignaal(signaal1,signaal2);
+    
+    //check if error is big enough to produce signal, else signal is 0 to save motors
+    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);
+    } else if (fabs(error1) > minimal_error && fabs(error2) <= minimal_error) {
+        verwerksignaal(signaal1,0);
+    } 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)
@@ -79,7 +96,16 @@
     error2_int = error2_int + Ts * error2;
     float signaal2=Kp*error2+Ki*error2_int;
 
-    verwerksignaal (signaal1,signaal2);
+     //check if error is big enough to produce signal, else signal is 0 to save motors
+    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);
+    } else if (fabs(error1) > minimal_error && fabs(error2) <= minimal_error) {
+        verwerksignaal(signaal1,0);
+    } else if (fabs(error1) <= minimal_error && fabs(error2) > minimal_error) {
+        verwerksignaal(0,signaal2);
+    }
 }
 
 void controlandadjust::PID(float error1, float error2, float Kp, float Ki,float Kd,float Ts,
@@ -99,7 +125,16 @@
     float signaal1= Kp * error1 + Ki * error1_int + Kd * error1_der;
     float signaal2= Kp * error2 + Ki * error2_int + Kd * error2_der;
 
-    verwerksignaal(signaal1,signaal2);
+     //check if error is big enough to produce signal, else signal is 0 to save motors
+    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);
+    } else if (fabs(error1) > minimal_error && fabs(error2) <= minimal_error) {
+        verwerksignaal(signaal1,0);
+    } else if (fabs(error1) <= minimal_error && fabs(error2) > minimal_error) {
+        verwerksignaal(0,signaal2);
+    }
 }
 
 void controlandadjust::STOP()
@@ -109,5 +144,5 @@
 }
 void controlandadjust::cutoff(float maxvalue)
 {
-   maxvaluepwm=maxvalue;
+    maxvaluepwm=maxvalue;
 }
\ No newline at end of file