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:
- 14:e1816efa712d
- Parent:
- 13:54ee98850a15
- Child:
- 15:3ebd0e666a9c
--- a/main.cpp Fri Oct 24 11:06:15 2014 +0000 +++ b/main.cpp Mon Oct 27 14:43:50 2014 +0000 @@ -62,9 +62,10 @@ Ticker ticker_regelaar; //Ticker voor regelaar motor Ticker ticker_EMG; //Ticker voor EMG meten +Timer biceps_kalibratie; //States definiëren -enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_OFFSET_BICEPS, EMG_BICEPS, KALIBRATIE_BICEPS, EMG_OFFSET_TRICEPS, EMG_TRICEPS, KALIBRATIE_TRICEPS}; //Alle states benoemen, ieder krijgt een getal beginnend met 0 +enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS}; //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 @@ -205,8 +206,8 @@ pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm2); if (pwm_to_motor_arm2 == 0) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer) - state = EMG_OFFSET_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel - pc.printf("KALIBRATIE_ARM2 afgerond\n"); //Tekst voor controle Arm 2 naar thuispositie + state = EMG_KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel + pc.printf("KALIBRATIE_ARM2 afgerond\n\r"); //Tekst voor controle Arm 2 naar thuispositie } } @@ -221,13 +222,12 @@ xbhp3 = xbhp2; xbhp2 = xbhp1; xbhp1 = xbhp; - xbn = xbhp; ybhp4 = ybhp3; ybhp3 = ybhp2; ybhp2 = ybhp1; ybhp1 = ybhp; - ybn1 = ybhp; //De vorige waarde in het notchfilter is de waarde uit het high pass filter + xbn = ybhp; //De input in het notchfilter is de waarde uit het high pass filter //Notch ybn = -C2*ybn1 - C3*ybn2 + D1*xbn + D2*xbn1 + D3*xbn2; //Filterfunctie in z-domein @@ -235,16 +235,15 @@ //Waarden voor de volgende ronde benoemen xbn2 = xbn1; xbn1 = xbn; - xblp = xbn; ybn2 = ybn1; ybn1 = ybn; - rb = ybn; + rb = ybn; //De input in de rectify is de waarde uit het notch filter //Rectify rbr = fabs(rb); //Absolute waarde van de waarde uit notchfilter - yblp1 = rbr; + xblp = rbr; //De input in het low pass filter is de waarde uit de rectify //Low pass yblp = -E2*yblp1 - E3*yblp2 - E4*yblp3 - E5*yblp4 + F1*xblp + F2*xblp1 + F3*xblp2 + F4*xblp3 + F5*xblp4; //Filterfunctie in z-domein @@ -253,13 +252,12 @@ xblp3 = xblp2; xblp2 = xblp1; xblp1 = xblp; - xbf = xblp; yblp4 = yblp3; yblp3 = yblp2; yblp2 = yblp1; yblp1 = yblp; - ybf = yblp; + xbf = yblp; //De uiteindelijk gefilterde EMG waarde is de output uit het low pass filter } @@ -275,14 +273,15 @@ lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm wait(2); //Twee seconden wachten - pc.printf("RUST afgerond\n"); //Tekst voor controle Aanzetten + pc.printf("RUST afgerond\n\r"); //Tekst voor controle Aanzetten state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel break; //Stopt acties in deze case } case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie - pc.printf("KALIBRATIE_ARM1\n"); + pc.printf("KALIBRATIE_ARM1\n\r"); wait(1); //Een seconde wachten + ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken while(state == KALIBRATIE_ARM1) { @@ -296,7 +295,7 @@ } case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie - pc.printf("KALIBRATIE_ARM1\n"); + pc.printf("KALIBRATIE_ARM1\n\r"); wait(1); //Een seconde wachten //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken @@ -311,67 +310,47 @@ ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer break; //Stopt acties in deze case } - - case EMG_OFFSET_BICEPS: { //Kalibratie EMG signaal biceps - pc.printf("EMG_OFFSET_BICEPS\n"); - wait(1); //Een seconde wachten - - ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconden de flag laten opsteken - - pc.printf("Ticker voor kalibratie compleet\n"); //Tekst voor controle Ticker voor kalibratie - - //5 seconden EMG biceps meten - - wait(1); //Een seconde wachten - lcd_r1 = " EMG kalibratie "; //Tekst op eerste regel van LCD scherm - lcd_r2 = " Span biceps aan"; //Tekst op tweede regel van LCD scherm - - while(state == EMG_OFFSET_BICEPS){ + + case EMG_KALIBRATIE_BICEPS: + { + pc.printf("EMG__KALIBRATIE_BICEPS\n\r"); + + lcd_r1 = " SPAN IN 5 SEC. "; + lcd_r2 = " 2 X BICEPS AAN "; + + ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); + biceps_kalibratie.start(); + + if(biceps_kalibratie.read() <= 5){ 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 + regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan - pc.printf("Ga EMG meten\n"); - - xbk = EMG_bi.read(); //EMG signaal uitlezen - xbk10 = xbk9; - xbk9 = xbk8; - xbk8 = xbk7; - xbk7 = xbk6; - xbk6 = xbk5; - xbk5 = xbk4; - xbk4 = xbk3; - xbk3 = xbk2; - xbk2 = xbk1; - xbk1 = xbk; - pc.printf("xbk10 is %f\n", xbk10); + xb = EMG_bi.read(); //EMG meten van biceps + + filter_biceps(); - if(xbk10 > 0){ - pc.printf("10 waarden gemeten\n"); - state = EMG_BICEPS; - } + if(biceps_kalibratie.read() == 1){ + lcd_r1 = " 4 "; + } + if(biceps_kalibratie.read() == 2){ + lcd_r1 = " 3 "; + } + if(biceps_kalibratie.read() == 3){ + lcd_r1 = " 2 "; + } + if(biceps_kalibratie.read() == 4){ + lcd_r1 = " 1 "; + } + } - break; - } - - case EMG_BICEPS: - { - pc.printf("EMG_BICEPS\n"); - meanxbk = (xbk1 + xbk2 + xbk3 + xbk4 + xbk5 + xbk6 + xbk7 + xbk8 + xbk9 + xbk10)/10; //Offset bepalen van de eerste 10 gemeten waarden - 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 meten van biceps - xbo = xb - meanxbk; //Gemeten waarden zonder offset - - filter_biceps(); - + state = EMG_KALIBRATIE_TRICEPS; } 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",state); + pc.printf("state: %u\n\r",state); } } \ No newline at end of file