![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Alles bij elkaar
Dependencies: Encoder HIDScope MODSERIAL TouchButton mbed-dsp mbed
Diff: main.cpp
- Revision:
- 7:b40bbf5be443
- Parent:
- 6:34da048d7168
- Child:
- 8:7bf274156e1f
--- a/main.cpp Thu Oct 30 14:50:35 2014 +0000 +++ b/main.cpp Thu Oct 30 15:16:04 2014 +0000 @@ -23,7 +23,7 @@ void Biceps(); void Calibratie_Triceps(); void Calibratie_Biceps(); -float pid(float setspeed, float measurement); +float pid(float setspeed, float measurement, bool reset = false); void motor2aansturing(); void motor1aansturing(); @@ -389,19 +389,24 @@ myled1=1; myled2=1; myled3=1; - } + } } } }//end int main -float pid(float setspeed, float measurement) +float pid(float setspeed, float measurement, bool reset) { float error; static float prev_error = 0; float out_p = 0; static float out_i = 0; float out_d = 0; + if(reset) + { + out_i = 0; + prev_error = 0; + } error = setspeed-measurement; out_p = error*K_P; out_i += error*K_I; @@ -612,7 +617,8 @@ { if (motor1.getPosition()>= ANGLEMIN && toestand != SLAAN) { - toestand = WACHTEN; + toestand = WACHTEN; + //out_i=0; 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 @@ -630,13 +636,14 @@ } if (toestand == SLAAN && motor1.getPosition() <= ANGLEMAX) { pc.printf("toestand = terugkeren\n\r"); + //out_i=0; 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"); - //pid(Vreturn, motor1.getSpeed()); - new_pwm=Vreturn; + new_pwm=pid(setspeed, motor1.getSpeed()); + pwm_motor1.write(new_pwm); pc.printf("new pwm %f\r\n",new_pwm); motordir1 = 0; @@ -646,7 +653,7 @@ pc.printf("ifwachten\n"); } if (toestand == SLAAN) { - new_pwm =setspeed; //pid(setspeed, motor1.getSpeed()); + pid(setspeed, motor1.getSpeed()); motordir1 = 1; pwm_motor1.write(new_pwm); pc.printf("SLAAN\n");