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:
- 31:36fe36657f8d
- Parent:
- 30:f79cf70e2917
- Child:
- 32:80fc2de5b511
--- a/main.cpp Tue Nov 04 00:29:03 2014 +0000 +++ b/main.cpp Tue Nov 04 00:33:35 2014 +0000 @@ -1319,29 +1319,32 @@ while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - 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 + ticker_motor_arm2_pid.detach(); + pwm_to_motor_arm2 = 1; + pwm_motor_arm2.write(pwm_to_motor_arm2); + dir_motor_arm2.write(0); - 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 + while(puls_motor_arm2.getPosition() < -175) { - if(puls_motor_arm2.getPosition() <= -175) { - wait(1); //Een seconde wachten - while(puls_motor_arm2.getPosition() < 0) { - referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar 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", pwm_to_motor_arm2); //PWM naar pc sturen + pc.printf("t is %f\n\r", t); //T naar pc sturen + } + pwm_to_motor_arm2 = 0.5; + pwm_motor_arm2.write(pwm_to_motor_arm2); + dir_motor_arm2.write(1); - 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 - } + while(puls_motor_arm2.getPosition() >= 0) { + 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\n\r"); //Staat stil naar pc sturen + state = WACHT; //Door naar de volgende state - if(puls_motor_arm2.getPosition() >= 0) { - pc.printf("staat stil\n\r"); //Staat stil naar pc sturen - state = WACHT; //Door naar de volgende state - } - } + } break; //Stop met alle acties in deze case } @@ -1372,29 +1375,32 @@ while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - referentie_arm2 = -0.5 * 150000 * t * t; //Referentie arm 2 loopt parabolisch af - t = t + 0.005; //Tijd loopt op met 0.005 per flag + ticker_motor_arm2_pid.detach(); + pwm_to_motor_arm2 = 1; + pwm_motor_arm2.write(pwm_to_motor_arm2); + dir_motor_arm2.write(0); - 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 + while(puls_motor_arm2.getPosition() < -175) { - if(puls_motor_arm2.getPosition() <= -175) { - wait(1); //Een seconde wachten - while(puls_motor_arm2.getPosition() < 0) { - referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar 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", pwm_to_motor_arm2); //PWM naar pc sturen + pc.printf("t is %f\n\r", t); //T naar pc sturen + } + pwm_to_motor_arm2 = 0.5; + pwm_motor_arm2.write(pwm_to_motor_arm2); + dir_motor_arm2.write(1); - 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 - } + while(puls_motor_arm2.getPosition() >= 0) { + 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\n\r"); //Staat stil naar pc sturen + state = WACHT; //Door naar de volgende state - if(puls_motor_arm2.getPosition() >= 0) { - pc.printf("staat stil\n\r"); //Staat stil naar pc sturen - state = WACHT; //Door naar de volgende state - } - } + } break; //Stop met alle acties in deze case @@ -1409,30 +1415,30 @@ while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - referentie_arm2 = -0.5 * 200000 * 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; + ticker_motor_arm2_pid.detach(); + pwm_to_motor_arm2 = 1; + pwm_motor_arm2.write(pwm_to_motor_arm2); + dir_motor_arm2.write(0); - 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 + while(puls_motor_arm2.getPosition() < -175) { - if(puls_motor_arm2.getPosition() <= -175) { - wait(1); //Een seconde wachten - while(puls_motor_arm2.getPosition() < 0) { - referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar 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", pwm_to_motor_arm2); //PWM naar pc sturen + pc.printf("t is %f\n\r", t); //T naar pc sturen + } + pwm_to_motor_arm2 = 0.5; + pwm_motor_arm2.write(pwm_to_motor_arm2); + dir_motor_arm2.write(1); - 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 - } - - if(puls_motor_arm2.getPosition() >= 0) { - pc.printf("staat stil\n\r"); //Staat stil naar pc sturen - state = WACHT; //Door naar de volgende state - } + while(puls_motor_arm2.getPosition() >= 0) { + 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\n\r"); //Staat stil naar pc sturen + state = WACHT; //Door naar de volgende state } break; }