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:
- 23:5267c928ae2b
- Parent:
- 22:838a17065bc7
- Child:
- 24:a1fdc830f96c
--- a/main.cpp Fri Oct 31 14:08:57 2014 +0000 +++ b/main.cpp Sun Nov 02 17:08:31 2014 +0000 @@ -9,7 +9,7 @@ #define KP_arm1 0.02 //Factor proprotionele regelaar arm 1 #define KI_arm1 0 //Factor integraal regelaar arm 1 #define KD_arm1 0 //Factor afgeleide regelaar arm 1 -#define KP_arm2 0.05 //Factor proprotionele regelaar arm 2 +#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 SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten @@ -46,7 +46,7 @@ PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1 DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1 -Encoder puls_motor_arm1(PTD0, PTD2); //Encoder pulsen tellen van motor arm 1, (geel, wit) +Encoder puls_motor_arm1(PTD2, PTD0); //Encoder pulsen tellen van motor arm 1, (geel, wit) PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2 DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2 Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2, (geel, wit) @@ -63,7 +63,7 @@ Timer EMG; //States definiëren -enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS, START, B, T, BB, BT, TB, TT, BBB, BBT, BTB, BTT, TBB, TBT, TTB, TTT, BBBB, BBBT, BBTB, BBTT, BTBB, BTBT, BTTB, BTTT, TBBB, TBBT, TBTB, TBTT, TTBB, TTBT, TTTB, TTTT}; //Alle states benoemen, ieder krijgt een getal beginnend met 0 +enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS, START, B, T, BB, BT, TB, TT, BBB, BBT, BTB, BTT, TBB, TBT, TTB, TTT, BBBB, BBBT, BBTB, BBTT, BTBB, BTBT, BTTB, BTTT, TBBB, TBBT, TBTB, TBTT, TTBB, TTBT, TTTB, TTTT, WACHT, THUISPOSITIE_MIDDEN}; //Alle states benoemen, ieder krijgt een getal beginnend met 0 uint8_t state = RUST; //State is rust aan het begin //Gedefinieerde datatypen en naamgeving en beginwaarden @@ -83,6 +83,8 @@ float referentie_arm1 = 0; float referentie_arm2 = 0; +float t = 0; + arm_biquad_casd_df1_inst_f32 highpass_biceps; //Highpass_biceps IIR filter in direct form 1 float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3, B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter float highpass_biceps_states[8]; //Aantal states van het filter, het aantal y(n-x) en x(n-x) @@ -150,9 +152,9 @@ keep_in_range(&pwm_to_motor_arm1, -1, 1); if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1 - dir_motor_arm1.write(1); + dir_motor_arm1.write(0); } else { //Anders richting nul - dir_motor_arm1.write(0); + dir_motor_arm1.write(1); } pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM @@ -270,20 +272,20 @@ 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 = referentie_arm2 + 35.0/200.0; + referentie_arm2 = referentie_arm2 + 123.0/200.0; pc.printf("pulsmotorgetposition2 %d ", puls_motor_arm2.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen pc.printf("pwmmotor2 %f ", pwm_to_motor_arm2); //Huidige PWM waarde naar motor naar pc sturen pc.printf("referentie2 %f\n\r", referentie_arm2); - if(referentie_arm2 >= 35) { - referentie_arm2 = 35; + if(referentie_arm2 >= 123) { + referentie_arm2 = 123; state = EMG_KALIBRATIE_BICEPS; } } wait(1); //Een seconde wachten - ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer + //ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer break; //Stopt acties in deze case } @@ -444,23 +446,22 @@ 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 - + EMG.start(); - - while(EMG.read() <= 3) - { + + while(EMG.read() <= 3) { while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - + EMG_meten(); } - + while(state == B) { while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - + EMG_meten(); - + if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = BB; //Ga door naar state BB } @@ -475,14 +476,13 @@ lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" T "); //Tekst op LCD scherm pc.printf("T\n\r"); //Controle naar pc sturen - + EMG.start(); - - while(EMG.read() <= 3) - { + + while(EMG.read() <= 3) { while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - + EMG_meten(); } @@ -506,15 +506,14 @@ lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" BB "); //Tekst op LCD scherm pc.printf("BB\n\r"); //Controle naar pc sturen - + EMG.reset(); EMG.start(); - - while(EMG.read() <= 3) - { + + while(EMG.read() <= 3) { while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - + EMG_meten(); } @@ -538,15 +537,14 @@ lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" BT "); //Tekst op LCD scherm pc.printf("BT\n\r"); //Controle naar pc sturen - + EMG.reset(); EMG.start(); - - while(EMG.read() <= 3) - { + + while(EMG.read() <= 3) { while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - + EMG_meten(); } @@ -570,15 +568,14 @@ lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" TB "); //Tekst op LCD scherm pc.printf("TB\n\r"); //Controle naar pc sturen - + EMG.reset(); EMG.start(); - - while(EMG.read() <= 3) - { + + while(EMG.read() <= 3) { while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - + EMG_meten(); } @@ -602,15 +599,14 @@ lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" TT "); //Tekst op LCD scherm pc.printf("TT\n\r"); //Controle naar pc sturen - + EMG.reset(); EMG.start(); - - while(EMG.read() <= 3) - { + + while(EMG.read() <= 3) { while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - + EMG_meten(); } @@ -634,15 +630,14 @@ lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" BBB "); //Tekst op LCD scherm pc.printf("BBB\n\r"); //Controle naar pc sturen - + EMG.reset(); EMG.start(); - - while(EMG.read() <= 3) - { + + while(EMG.read() <= 3) { while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - + EMG_meten(); } @@ -667,15 +662,14 @@ lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" BBT "); //Tekst op LCD scherm pc.printf("BBT\n\r"); //Controle naar pc sturen - + EMG.reset(); EMG.start(); - - while(EMG.read() <= 3) - { + + while(EMG.read() <= 3) { while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - + EMG_meten(); } @@ -699,15 +693,14 @@ lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" BTB "); //Tekst op LCD scherm pc.printf("BTB\n\r"); //Controle naar pc sturen - + EMG.reset(); EMG.start(); - - while(EMG.read() <= 3) - { + + while(EMG.read() <= 3) { while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - + EMG_meten(); } @@ -731,15 +724,14 @@ lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" BTT "); //Tekst op LCD scherm pc.printf("BTT\n\r"); //Controle naar pc sturen - + EMG.reset(); EMG.start(); - - while(EMG.read() <= 3) - { + + while(EMG.read() <= 3) { while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - + EMG_meten(); } @@ -763,15 +755,14 @@ lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" TBB "); //Tekst op LCD scherm pc.printf("TBB\n\r"); //Controle naar pc sturen - + EMG.reset(); EMG.start(); - - while(EMG.read() <= 3) - { + + while(EMG.read() <= 3) { while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - + EMG_meten(); } @@ -795,15 +786,14 @@ lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" TBT "); //Tekst op LCD scherm pc.printf("TBT\n\r"); //Controle naar pc sturen - + EMG.reset(); EMG.start(); - - while(EMG.read() <= 3) - { + + while(EMG.read() <= 3) { while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - + EMG_meten(); } @@ -827,15 +817,14 @@ lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" TTB "); //Tekst op LCD scherm pc.printf("TTB\n\r"); //Controle naar pc sturen - + EMG.reset(); EMG.start(); - - while(EMG.read() <= 3) - { + + while(EMG.read() <= 3) { while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - + EMG_meten(); } @@ -859,15 +848,14 @@ lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" TTT "); //Tekst op LCD scherm pc.printf("TTT\n\r"); //Controle naar pc sturen - + EMG.reset(); EMG.start(); - - while(EMG.read() <= 3) - { + + while(EMG.read() <= 3) { while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - + EMG_meten(); } @@ -1008,83 +996,248 @@ break; //Stop met alle acties in deze case } - case TBTT: { + case TBTT: { //Motoraansturing voor richten op doel middenboven lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" TBTT "); //Tekst op LCD scherm pc.printf("TBTT\n\r"); //Controle naar pc sturen while(state == TBTT) { //Motoractie - } - break; //Stop met alle acties in deze case - } - - case TTBB: { - 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 - } - break; //Stop met alle acties in deze case - } - - case TTBT: { - 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 - } - break; //Stop met alle acties in deze case - } - - case TTTB: { - 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 - } - break; //Stop met alle acties in deze case - } - - case TTTT: { - lcd.locate(0,0); //Zet tekst op eerste regel - lcd.printf(" TTTT "); //Tekst op LCD scherm - pc.printf("TTTT\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 + while(puls_motor_arm1.getPosition() > 48) { + referentie_arm1 = referentie_arm1 - 132.0/200.0; + pc.printf("referentie arm 1 %f ", referentie_arm1); + pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); + pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); + } + pc.printf("arm 1 op positie"); - referentie_arm2 = referentie_arm2 + 0.1902; + while(puls_motor_arm2.getPosition() < 167) { + referentie_arm2 = referentie_arm2 + 44.0/200.0; + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); + } + pc.printf("arm2 op positie"); + + + if(referentie_arm2 >= 167) { + while(puls_motor_arm2.getPosition() > -8) { + + + + 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() <= -8) { + wait(1); + while(puls_motor_arm2.getPosition() > -100) { + 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 + } - if(referentie_arm2 >= 9.51) { - while(pwm_to_motor_arm2 != 0) { - referentie_arm2 = referentie_arm2 - 90/200; + 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) { + pc.printf("staat stil\n\r"); + state = WACHT; + } + } - if(pwm_to_motor_arm2 == 0) { - 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 * 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; + } + } } } - } - 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 - } + + 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; + } - pc.printf("state: %u\n\r",state); + 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 + } + + pc.printf("state: %u\n\r",state); + } + //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) + } } - //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) } -} - +} \ No newline at end of file