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:
- 24:a1fdc830f96c
- Parent:
- 23:5267c928ae2b
- Child:
- 25:71e52c56be13
--- a/main.cpp Sun Nov 02 17:08:31 2014 +0000 +++ b/main.cpp Sun Nov 02 19:42:31 2014 +0000 @@ -1043,201 +1043,198 @@ if(puls_motor_arm2.getPosition() <= -8) { wait(1); - while(puls_motor_arm2.getPosition() > -100) { + while(puls_motor_arm2.getPosition() < 167) { referentie_arm2 = referentie_arm2 + 167.0/200.0; pc.printf("referentie arm 2 %f ", referentie_arm2); pc.printf("get position %d ", puls_motor_arm2.getPosition()); pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); - - - - if(puls_motor_arm2.getPosition() >= 167) { - pc.printf("staat stil\n\r"); - state = WACHT; - } - - } - } - } - break; //Stop met alle acties in deze case - } - - case TTBB: { //Motoraansturing voor richten op doel linksonder - lcd.locate(0,0); //Zet tekst op eerste regel - lcd.printf(" TTBB "); //Tekst op LCD scherm - pc.printf("TTBB\n\r"); //Controle naar pc sturen - - while(state == TTBB) { - //Motoractie - 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 - - //Positie arm 1 is goed - //Snelheid arm 2 - - referentie_arm2 = 0.5 * 200000 * t * t; - t = t + 0.005; - - pc.printf("referentie arm 2 %f ", referentie_arm2); - pc.printf("get position %d ", puls_motor_arm2.getPosition()); - pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); - pc.printf("t is %f\n\r", t); - - if(puls_motor_arm2.getPosition() >= 175) { - wait(1); - while(puls_motor_arm2.getPosition() > 0) { - referentie_arm2 = referentie_arm2 - 175.0/200.0; - - - pc.printf("referentie arm 2 %f ", referentie_arm2); - pc.printf("get position %d ", puls_motor_arm2.getPosition()); - pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); } - - if(puls_motor_arm2.getPosition() <= 0) { - pc.printf("staat stil\n\r"); - state = WACHT; - } - - } - } - break; //Stop met alle acties in deze case - } - - case TTBT: { //Motoraansturing voor terug naar kalibratiepositie - lcd.locate(0,0); //Zet tekst op eerste regel - lcd.printf(" TTBT "); //Tekst op LCD scherm - pc.printf("TTBT\n\r"); //Controle naar pc sturen - - while(state == TTBT) { - //Motoractie - lcd.printf(" GEEN DOEL "); - wait(1); - state = WACHT; - } - break; //Stop met alle acties in deze case - } - - case TTTB: { //Motoraansturing voor richten op doel linksmidden - lcd.locate(0,0); //Zet tekst op eerste regel - lcd.printf(" TTTB "); //Tekst op LCD scherm - pc.printf("TTTB\n\r"); //Controle naar pc sturen - - while(state == TTTB) { - //Motoractie - - 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 - - //Positie arm 1 is goed - //Snelheid arm 2 - - referentie_arm2 = 0.5 * 400000 * t * t; - t = t + 0.005; - - pc.printf("referentie arm 2 %f ", referentie_arm2); - pc.printf("get position %d ", puls_motor_arm2.getPosition()); - pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); - pc.printf("t is %f\n\r", t); - - if(puls_motor_arm2.getPosition() >= 175) { - wait(1); - while(puls_motor_arm2.getPosition() > 0) { - referentie_arm2 = referentie_arm2 - 175.0/200.0; - - - pc.printf("referentie arm 2 %f ", referentie_arm2); - pc.printf("get position %d ", puls_motor_arm2.getPosition()); - pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); - } - - - if(puls_motor_arm2.getPosition() <= 0) { + if(puls_motor_arm2.getPosition() >= 167) { pc.printf("staat stil\n\r"); state = WACHT; } - - } - } - - break; //Stop met alle acties in deze case - } - - - case TTTT: { //Motoraansturing voor richten op doel linksboven - lcd.locate(0,0); //Zet tekst op eerste regel - lcd.printf(" TTTT "); //Tekst op LCD scherm - pc.printf("TTBB\n\r"); //Controle naar pc sturen - - while(state == TTTT) { - 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 - - //Positie arm 1 is goed - //Snelheid arm 2 - - referentie_arm2 = 0.5 * 600000 * t * t; - t = t + 0.005; - - pc.printf("referentie arm 2 %f ", referentie_arm2); - pc.printf("get position %d ", puls_motor_arm2.getPosition()); - pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); - pc.printf("t is %f\n\r", t); - - if(puls_motor_arm2.getPosition() >= 175) { - wait(1); - while(puls_motor_arm2.getPosition() > 0) { - referentie_arm2 = referentie_arm2 - 175.0/200.0; - - - pc.printf("referentie arm 2 %f ", referentie_arm2); - pc.printf("get position %d ", puls_motor_arm2.getPosition()); - pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); - } - - - if(puls_motor_arm2.getPosition() <= 0) { - pc.printf("staat stil\n\r"); - state = WACHT; - } - } } } + break; //Stop met alle acties in deze case + } - case THUISPOSITIE_MIDDEN: { - while(puls_motor_arm2.getPosition() > 123) { - referentie_arm2 = referentie_arm2 - 44.0/200.0; + case TTBB: { //Motoraansturing voor richten op doel linksonder + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TTBB "); //Tekst op LCD scherm + pc.printf("TTBB\n\r"); //Controle naar pc sturen + + while(state == TTBB) { + //Motoractie + 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 + + //Positie arm 1 is goed + //Snelheid arm 2 + + referentie_arm2 = 0.5 * 100000 * t * t; + t = t + 0.005; + + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); + pc.printf("t is %f\n\r", t); + + if(puls_motor_arm2.getPosition() >= 175) { + wait(1); + while(puls_motor_arm2.getPosition() > 0) { + referentie_arm2 = referentie_arm2 - 175.0/200.0; + + + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); + } + + + if(puls_motor_arm2.getPosition() <= 0) { + pc.printf("staat stil\n\r"); + state = WACHT; + } + } + } + break; //Stop met alle acties in deze case + } - while(puls_motor_arm1.getPosition() < 180) { - referentie_arm1 = referentie_arm1 + 132.0/200.0; + case TTBT: { //Motoraansturing voor terug naar kalibratiepositie + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TTBT "); //Tekst op LCD scherm + pc.printf("TTBT\n\r"); //Controle naar pc sturen + + while(state == TTBT) { + //Motoractie + lcd.printf(" GEEN DOEL "); + wait(1); + state = WACHT; + } + break; //Stop met alle acties in deze case + } + + case TTTB: { //Motoraansturing voor richten op doel linksmidden + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TTTB "); //Tekst op LCD scherm + pc.printf("TTTB\n\r"); //Controle naar pc sturen + + while(state == TTTB) { + //Motoractie + + 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 + + //Positie arm 1 is goed + //Snelheid arm 2 + + referentie_arm2 = 0.5 * 150000 * t * t; + t = t + 0.005; + + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); + pc.printf("t is %f\n\r", t); + + if(puls_motor_arm2.getPosition() >= 175) { + wait(1); + while(puls_motor_arm2.getPosition() > 0) { + referentie_arm2 = referentie_arm2 - 175.0/200.0; + + + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); + } + + + if(puls_motor_arm2.getPosition() <= 0) { + pc.printf("staat stil\n\r"); + state = WACHT; + } + } - - state = WACHT; - break; } - case WACHT: { - lcd.printf(" WACHT "); - wait(5); - state = START; + break; //Stop met alle acties in deze case + } + + + case TTTT: { //Motoraansturing voor richten op doel linksboven + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TTTT "); //Tekst op LCD scherm + pc.printf("TTBB\n\r"); //Controle naar pc sturen + + while(state == TTTT) { + 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 + + //Positie arm 1 is goed + //Snelheid arm 2 + + referentie_arm2 = 0.5 * 200000 * t * t; + t = t + 0.005; + + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); + pc.printf("t is %f\n\r", t); + + if(puls_motor_arm2.getPosition() >= 175) { + wait(1); + while(puls_motor_arm2.getPosition() > 0) { + referentie_arm2 = referentie_arm2 - 175.0/200.0; + + + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); + } + + + if(puls_motor_arm2.getPosition() <= 0) { + pc.printf("staat stil\n\r"); + state = WACHT; + } + + } + } + } + + case THUISPOSITIE_MIDDEN: { + while(puls_motor_arm2.getPosition() > 123) { + referentie_arm2 = referentie_arm2 - 44.0/200.0; + } + + while(puls_motor_arm1.getPosition() < 180) { + referentie_arm1 = referentie_arm1 + 132.0/200.0; + } + + state = WACHT; + break; + } + + case WACHT: { + lcd.printf(" WACHT "); + wait(5); + state = START; - default: { - //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case - state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan - } + default: { + //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case + state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan + } - pc.printf("state: %u\n\r",state); - } - //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) + pc.printf("state: %u\n\r",state); } + //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) } } -} \ No newline at end of file +}