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:
- 22:838a17065bc7
- Parent:
- 21:f4e9f6c28de1
- Child:
- 23:5267c928ae2b
--- a/main.cpp Fri Oct 31 12:37:06 2014 +0000 +++ b/main.cpp Fri Oct 31 14:08:57 2014 +0000 @@ -60,6 +60,7 @@ Timer triceps_kalibratie; //Timer voor kalibratiemeting EMG triceps Ticker ticker_motor_arm1_pid; Ticker ticker_motor_arm2_pid; +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 @@ -201,6 +202,15 @@ } +void EMG_meten() +{ + xb = EMG_bi.read(); //EMG signaal biceps uitlezen + filter_biceps(); //EMG signaal biceps filteren + xt = EMG_tri.read(); //EMG signaal triceps uitlezen + filter_triceps(); //EMG signaal triceps filteren +} + + int main() //Main script { ticker_motor_arm1_pid.attach(motor_arm1_pid,SAMPLETIME_REGELAAR); @@ -435,19 +445,22 @@ lcd.printf(" B "); //Tekst op LCD scherm pc.printf("B\n\r"); //Controle naar pc sturen - wait(3); - + EMG.start(); + + 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 - xb = 0; - xbf = 0; - xb = EMG_bi.read(); //EMG signaal biceps uitlezen - filter_biceps(); //EMG signaal biceps filteren - xt = EMG_tri.read(); //EMG signaal triceps uitlezen - filter_triceps(); //EMG signaal triceps filteren - + 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 } @@ -463,16 +476,21 @@ lcd.printf(" T "); //Tekst op LCD scherm pc.printf("T\n\r"); //Controle naar pc sturen - wait(3); + EMG.start(); + + 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 == T) { 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 - xb = EMG_bi.read(); //EMG signaal biceps uitlezen - filter_biceps(); //EMG signaal biceps filteren - xt = EMG_tri.read(); //EMG signaal triceps uitlezen - filter_triceps(); //EMG signaal triceps filteren + EMG_meten(); if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = TB; //Ga door naar state TB @@ -489,17 +507,22 @@ lcd.printf(" BB "); //Tekst op LCD scherm pc.printf("BB\n\r"); //Controle naar pc sturen - wait(3); + EMG.reset(); + EMG.start(); + + 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 == BB) { 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 - xb = EMG_bi.read(); //EMG signaal biceps uitlezen - xbf = 0; - filter_biceps(); //EMG signaal biceps filteren - xt = EMG_tri.read(); //EMG signaal triceps uitlezen - filter_triceps(); //EMG signaal triceps filteren + EMG_meten(); if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = BBB; //Ga door naar state BBB @@ -516,16 +539,22 @@ lcd.printf(" BT "); //Tekst op LCD scherm pc.printf("BT\n\r"); //Controle naar pc sturen - wait(3); + EMG.reset(); + EMG.start(); + + 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 == BT) { 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 - xb = EMG_bi.read(); //EMG signaal biceps uitlezen - filter_biceps(); //EMG signaal biceps filteren - xt = EMG_tri.read(); //EMG signaal triceps uitlezen - filter_triceps(); //EMG signaal triceps filteren + EMG_meten(); if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = BTB; //Ga door naar state BTB @@ -542,16 +571,22 @@ lcd.printf(" TB "); //Tekst op LCD scherm pc.printf("TB\n\r"); //Controle naar pc sturen - wait(3); + EMG.reset(); + EMG.start(); + + 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 == TB) { 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 - xb = EMG_bi.read(); //EMG signaal biceps uitlezen - filter_biceps(); //EMG signaal biceps filteren - xt = EMG_tri.read(); //EMG signaal triceps uitlezen - filter_triceps(); //EMG signaal triceps filteren + EMG_meten(); if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = TBB; //Ga door naar state TBB @@ -568,16 +603,22 @@ lcd.printf(" TT "); //Tekst op LCD scherm pc.printf("TT\n\r"); //Controle naar pc sturen - wait(3); + EMG.reset(); + EMG.start(); + + 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 == TT) { 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 - xb = EMG_bi.read(); //EMG signaal biceps uitlezen - filter_biceps(); //EMG signaal biceps filteren - xt = EMG_tri.read(); //EMG signaal triceps uitlezen - filter_triceps(); //EMG signaal triceps filteren + EMG_meten(); if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = TTB; //Ga door naar state TTB @@ -594,16 +635,22 @@ lcd.printf(" BBB "); //Tekst op LCD scherm pc.printf("BBB\n\r"); //Controle naar pc sturen - wait(3); + EMG.reset(); + EMG.start(); + + 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 == BBB) { 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 - xb = EMG_bi.read(); //EMG signaal biceps uitlezen - filter_biceps(); //EMG signaal biceps filteren - xt = EMG_tri.read(); //EMG signaal triceps uitlezen - filter_triceps(); //EMG signaal triceps filteren + EMG_meten(); if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = BBBB; //Ga door naar state BBBB @@ -621,16 +668,22 @@ lcd.printf(" BBT "); //Tekst op LCD scherm pc.printf("BBT\n\r"); //Controle naar pc sturen - wait(3); + EMG.reset(); + EMG.start(); + + 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 == BBT) { 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 - xb = EMG_bi.read(); //EMG signaal biceps uitlezen - filter_biceps(); //EMG signaal biceps filteren - xt = EMG_tri.read(); //EMG signaal triceps uitlezen - filter_triceps(); //EMG signaal triceps filteren + EMG_meten(); if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = BBTB; //Ga door naar state BBTB @@ -647,16 +700,22 @@ lcd.printf(" BTB "); //Tekst op LCD scherm pc.printf("BTB\n\r"); //Controle naar pc sturen - wait(3); + EMG.reset(); + EMG.start(); + + 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 == BTB) { 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 - xb = EMG_bi.read(); //EMG signaal biceps uitlezen - filter_biceps(); //EMG signaal biceps filteren - xt = EMG_tri.read(); //EMG signaal triceps uitlezen - filter_triceps(); //EMG signaal triceps filteren + EMG_meten(); if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = BTBB; //Ga door naar state BTBB @@ -673,16 +732,22 @@ lcd.printf(" BTT "); //Tekst op LCD scherm pc.printf("BTT\n\r"); //Controle naar pc sturen - wait(3); + EMG.reset(); + EMG.start(); + + 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 == BTT) { 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 - xb = EMG_bi.read(); //EMG signaal biceps uitlezen - filter_biceps(); //EMG signaal biceps filteren - xt = EMG_tri.read(); //EMG signaal triceps uitlezen - filter_triceps(); //EMG signaal triceps filteren + EMG_meten(); if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = BTTB; //Ga door naar state BTTB @@ -699,16 +764,22 @@ lcd.printf(" TBB "); //Tekst op LCD scherm pc.printf("TBB\n\r"); //Controle naar pc sturen - wait(3); + EMG.reset(); + EMG.start(); + + 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 == TBB) { 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 - xb = EMG_bi.read(); //EMG signaal biceps uitlezen - filter_biceps(); //EMG signaal biceps filteren - xt = EMG_tri.read(); //EMG signaal triceps uitlezen - filter_triceps(); //EMG signaal triceps filteren + EMG_meten(); if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = TBBB; //Ga door naar state TBBB @@ -725,16 +796,22 @@ lcd.printf(" TBT "); //Tekst op LCD scherm pc.printf("TBT\n\r"); //Controle naar pc sturen - wait(3); + EMG.reset(); + EMG.start(); + + 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 == TBT) { 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 - xb = EMG_bi.read(); //EMG signaal biceps uitlezen - filter_biceps(); //EMG signaal biceps filteren - xt = EMG_tri.read(); //EMG signaal triceps uitlezen - filter_triceps(); //EMG signaal triceps filteren + EMG_meten(); if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = TBTB; //Ga door naar state TBTB @@ -748,19 +825,25 @@ case TTB: { 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 + lcd.printf(" TTB "); //Tekst op LCD scherm + pc.printf("TTB\n\r"); //Controle naar pc sturen + + EMG.reset(); + EMG.start(); - wait(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 == BBB) { + while(state == TTB) { 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 - xb = EMG_bi.read(); //EMG signaal biceps uitlezen - filter_biceps(); //EMG signaal biceps filteren - xt = EMG_tri.read(); //EMG signaal triceps uitlezen - filter_triceps(); //EMG signaal triceps filteren + EMG_meten(); if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = TTBB; //Ga door naar state TTBB @@ -777,16 +860,22 @@ lcd.printf(" TTT "); //Tekst op LCD scherm pc.printf("TTT\n\r"); //Controle naar pc sturen - wait(3); + EMG.reset(); + EMG.start(); + + 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 == TTT) { 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 - xb = EMG_bi.read(); //EMG signaal biceps uitlezen - filter_biceps(); //EMG signaal biceps filteren - xt = EMG_tri.read(); //EMG signaal triceps uitlezen - filter_triceps(); //EMG signaal triceps filteren + EMG_meten(); if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = TTTB; //Ga door naar state TTTB @@ -910,10 +999,10 @@ case TBTB: { lcd.locate(0,0); //Zet tekst op eerste regel - lcd.printf(" TBBB "); //Tekst op LCD scherm - pc.printf("TBBB\n\r"); //Controle naar pc sturen + lcd.printf(" TBTB "); //Tekst op LCD scherm + pc.printf("TBTB\n\r"); //Controle naar pc sturen - while(state == TBBB) { + while(state == TBTB) { //Motoractie } break; //Stop met alle acties in deze case