Diff: main.cpp

Revision:

11:d1260f8e5300

Parent:

9:ba7f541cef3a

diff -r ba7f541cef3a -r d1260f8e5300 main.cpp
--- a/main.cpp	Thu Oct 30 16:24:22 2014 +0000
+++ b/main.cpp	Thu Oct 30 20:11:54 2014 +0000
@@ -23,7 +23,7 @@
 void Biceps();
 void Calibratie_Triceps();
 void Calibratie_Biceps();
-float pid(float setspeed, float measurement, bool reset = false);
+float pid(float setspeed, float measurement);
 void motor2aansturing();
 void motor1aansturing();
 
@@ -105,6 +105,7 @@
 
 enum standen {STAND1=0, STAND2=1, STAND3=2};
 standen hoek2 = STAND1;
+static float    out_i = 0;
 
 int main ()
 {
@@ -395,18 +396,12 @@
 }//end int main
 
 
-float pid(float setspeed, float measurement, bool reset )
+float pid(float setspeed, float measurement)
 {
     float error;
     static float prev_error = 0;
     float           out_p = 0;
-    static float    out_i = 0;
     float           out_d = 0;
-    //if(reset==true)
-    //{
-    //    out_i = 0;
-    //    prev_error = 0;
-    //}
     error  = setspeed-measurement;
     out_p  = error*K_P;
     out_i += error*K_I;
@@ -618,8 +613,7 @@
 
     if (motor1.getPosition()>= ANGLEMIN && toestand != SLAAN) {
         toestand = WACHTEN;
-        //out_i=0;
-        //pid(0,0,true); 
+        out_i = 0; // resets integrator control
         pc.printf("if1\n");
     }
     if (snelheidsstand != 0 && toestand == WACHTEN && stop == 0) { // stop = 0 in het begin let op dat dit na reset nog zo is
@@ -637,14 +631,13 @@
     }
     if (toestand == SLAAN && motor1.getPosition() <= ANGLEMAX) {
         pc.printf("toestand = terugkeren\n\r");
-        //out_i=0;
-        //pid(0,0,true);
+        out_i = 0; // resets integrator control
         toestand = TERUGKEREN; 
         stop = 1; //zorgt dat hij niet weer gaat slaan tot er gereset is...
     }
     if (toestand == TERUGKEREN) {
         pc.printf("motor gaat terugkeren\n\r");
-        new_pwm=pid(setspeed, motor1.getSpeed(),false);
+        new_pwm=pid(setspeed, motor1.getSpeed());
         
         pwm_motor1.write(new_pwm);
         pc.printf("new pwm %f\r\n",new_pwm);
@@ -655,7 +648,7 @@
         pc.printf("ifwachten\n");
     }
     if (toestand == SLAAN) {
-        pid(setspeed, motor1.getSpeed(),false);
+        pid(setspeed, motor1.getSpeed());
         motordir1 = 1;
         pwm_motor1.write(new_pwm);
         pc.printf("SLAAN\n");