2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range
Dependencies: Encoder MODSERIAL TextLCD mbed mbed-dsp
Diff: main.cpp
- Revision:
- 29:f95f0cc84349
- Parent:
- 28:e2f5cee5e73b
- Child:
- 30:f79cf70e2917
diff -r e2f5cee5e73b -r f95f0cc84349 main.cpp --- a/main.cpp Mon Nov 03 21:12:14 2014 +0000 +++ b/main.cpp Mon Nov 03 23:03:59 2014 +0000 @@ -12,7 +12,7 @@ #define KD_arm1 0 //Factor afgeleide regelaar arm 1 #define KP_arm2 0.01 //Factor proprotionele regelaar arm 2 #define KI_arm2 0 //Factor integraal regelaar arm 2 -#define KD_arm2 0 //Factor afgeleide regelaar arm 2 +#define KD_arm2 0.0008 //Factor afgeleide regelaar arm 2 //Filtercoëfficiënten per filter, allemaal 2e orde filters, zo cascade van hogere orde mogelijk //High Pass filter @@ -448,6 +448,8 @@ } case B: { //Tweede keuze maken voor doel + state = BBBB; + break; lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" B "); //Tekst op LCD scherm pc.printf("B\n\r"); //Controle naar pc sturen @@ -897,46 +899,47 @@ pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } - - if(puls_motor_arm1.getPosition() <= -84){ + + if(puls_motor_arm1.getPosition() <= -84) { referentie_arm1 = -84; } + + while(puls_motor_arm2.getPosition() < 211) { referentie_arm2 = referentie_arm2 + 88.0/200.0; //Referentie arm 2 loopt op in een seconde naar de gewenste waarde pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen } - + ticker_motor_arm2_pid.detach(); + pwm_to_motor_arm2 = 1; + pwm_motor_arm2.write(pwm_to_motor_arm2); + dir_motor_arm2.write(0); while(puls_motor_arm2.getPosition() > 36) { - referentie_arm2 = -0.5 * 100000 * t * t; //Referentie arm 2 loopt parabolisch af - t = t + 0.005; //Tijd loopt op met 0.005 per flag - pwm_motor_arm1 = 0.075; - pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen pc.printf("t is %f\n\r", t); //T naar pc sturen } - - if(puls_motor_arm2.getPosition() <= 36) { - wait(1); //Een seconde wachten - while(puls_motor_arm2.getPosition() < 211) { - referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar de gewenste waarde + pwm_to_motor_arm2 = 0.5; + pwm_motor_arm2.write(pwm_to_motor_arm2); + dir_motor_arm2.write(1); + while(puls_motor_arm2.getPosition() <= 211) { - pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen - pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen - pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen - } + pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen + pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen + pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen + } + ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); + pc.printf("staat stil"); - if(puls_motor_arm2.getPosition() >= 211) { - pc.printf("staat stil\n\r"); //Staat stil naar pc sturen - state = THUISPOSITIE_RECHTS; //Door naar de volgende state - } - } + state = THUISPOSITIE_RECHTS; //Door naar de volgende state + } + + break; //Stop met alle acties in deze case } @@ -971,8 +974,8 @@ pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } - - if(puls_motor_arm1.getPosition() <= -84){ + + if(puls_motor_arm1.getPosition() <= -84) { referentie_arm1 = -84; } @@ -1027,8 +1030,8 @@ pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } - - if(puls_motor_arm1.getPosition() <= -84){ + + if(puls_motor_arm1.getPosition() <= -84) { referentie_arm1 = -84; } @@ -1147,8 +1150,8 @@ pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } - - if(puls_motor_arm1.getPosition() <= 48){ + + if(puls_motor_arm1.getPosition() <= 48) { referentie_arm1 = 48; } @@ -1219,8 +1222,8 @@ pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } - - if(puls_motor_arm1.getPosition() <= 48){ + + if(puls_motor_arm1.getPosition() <= 48) { referentie_arm1 = 48; } @@ -1275,8 +1278,8 @@ pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } - - if(puls_motor_arm1.getPosition() <= 48){ + + if(puls_motor_arm1.getPosition() <= 48) { referentie_arm1 = 48; }