![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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:
- 15:3ebd0e666a9c
- Parent:
- 14:e1816efa712d
- Child:
- 16:c34c5d9dfe1a
diff -r e1816efa712d -r 3ebd0e666a9c main.cpp --- a/main.cpp Mon Oct 27 14:43:50 2014 +0000 +++ b/main.cpp Tue Oct 28 11:11:13 2014 +0000 @@ -2,30 +2,27 @@ #include "MODSERIAL.h" //MODSERIAL bibliotheek inladen, communicatie met PC #include "encoder.h" //Encoder bibliotheek inladen, communicatie met encoder #include "TextLCD.h" //LCD scherm bibliotheek inladen, communicatie met LCD scherm +#include "arm_math.h" //Filter bibliotheek inladen, makkelijk de filters maken, minder grote lappen tekst //Constanten definiëren en waarde geven #define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor -#define KP_arm1 0.3 //Factor proprotionele regelaar arm 1 +#define KP_arm1 0.01 //Factor proprotionele regelaar arm 1 #define KI_arm1 0 //Factor integraal regelaar arm 1 -#define KD_arm1 0.02 //Factor afgeleide regelaar arm 1 -#define KP_arm2 0.001 //Factor proprotionele regelaar arm 2 +#define KD_arm1 0 //Factor afgeleide regelaar arm 1 +#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 KD_arm2 0 //Factor afgeleide regelaar arm 2 #define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten #define PULS_ARM1_HOME_KALIBRATIE 180 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft #define PULS_ARM2_HOME_KALIBRATIE 393 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft //High Pass filter Filtercoëfficiënten -#define A1 1 -#define A2 -3.1806 -#define A3 3.8612 -#define A4 -2.1122 -#define A5 0.4383 -#define B1 4.1660E-4 -#define B2 0.0017 -#define B3 0.0025 -#define B4 0.0017 -#define B5 4.1660E-4 +#define A1 1 +#define A2 -1.5610 +#define A3 0.6414 +#define B1 0.0201 +#define B2 0.0402 +#define B3 0.0201 //Notch filter Filtercoëfficiënten #define C1 1 @@ -37,15 +34,12 @@ //Low pass filter Filtercoëfficiënten #define E1 1 -#define E2 3.9179 -#define E3 5.7571 -#define E4 3.7603 -#define E5 0.9212 -#define F1 0.9598 -#define F2 3.8391 -#define F3 5.7587 -#define F4 3.8391 -#define F5 0.9598 +#define E2 -1.9645 +#define E3 0.9651 +#define F1 1.5515E-4 +#define F2 3.1030E-4 +#define F3 1.5515E-4 + //Pinverdeling en naamgeving variabelen TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm @@ -58,14 +52,16 @@ 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) AnalogIn EMG_bi(PTB1); //Meten EMG signaal biceps +AnalogIn EMG_tri(PTB2); //Blauw op 3,3 V en groen op GND Ticker ticker_regelaar; //Ticker voor regelaar motor Ticker ticker_EMG; //Ticker voor EMG meten Timer biceps_kalibratie; +Timer triceps_kalibratie; //States definiëren -enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_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, BEGIN_METEN, B, T}; //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 @@ -82,64 +78,41 @@ float vorige_error_arm2 = 0; //Derivative actie van regelaar arm 2 float integraal_arm2 = 0; //Integraal actie van regelaar arm 2 float afgeleide_arm2; //Afgeleide actie van regelaar arm 2 -float xbk = 0; //Gemeten EMG waarde biceps in de kalibratie -float xbk1 = 0; //Gemeten EMG waarde biceps in de kalibratie -float xbk2 = 0; //Gemeten EMG waarde biceps in de kalibratie -float xbk3 = 0; //Gemeten EMG waarde biceps in de kalibratie -float xbk4 = 0; //Gemeten EMG waarde biceps in de kalibratie -float xbk5 = 0; //Gemeten EMG waarde biceps in de kalibratie -float xbk6 = 0; //Gemeten EMG waarde biceps in de kalibratie -float xbk7 = 0; //Gemeten EMG waarde biceps in de kalibratie -float xbk8 = 0; //Gemeten EMG waarde biceps in de kalibratie -float xbk9 = 0; //Gemeten EMG waarde biceps in de kalibratie -float xbk10 = 0; //Gemeten EMG waarde biceps in de kalibratie -float meanxbk; //Offset van biceps kalibratie float xb; //Gemeten EMG waarde biceps -float xbo; //Gemeten EMG waarde biceps min de offset, deze gaat in de filters +float xt; + +arm_biquad_casd_df1_inst_f32 highpass_biceps; +float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3}; +float highpass_biceps_states[4]; -//High Pass filter Y(n-x) waarden instellen op nul en definiëren als float -float xbhp; //Zelfde als xbo, maar makkelijker in notatie in filter -float xbhp1 = 0; -float xbhp2 = 0; -float xbhp3 = 0; -float xbhp4 = 0; -float xbhp5 = 0; -float ybhp; -float ybhp1 = 0; -float ybhp2 = 0; -float ybhp3 = 0; -float ybhp4 = 0; -float ybhp5 = 0; +arm_biquad_casd_df1_inst_f32 notch_biceps; +float notch_biceps_const[] = {D1, D2, D3, -C2, -C3}; +float notch_biceps_states[4]; + +arm_biquad_casd_df1_inst_f32 lowpass_biceps; +float lowpass_biceps_const[] = {F1, F2, F3, -E2, -E3}; +float lowpass_biceps_states[4]; + +float xbf; +float xbfmax = 0; -//Notch filter -float xbn; -float xbn1 = 0; -float xbn2 = 0; -float ybn; -float ybn1 = 0; -float ybn2 = 0; +arm_biquad_casd_df1_inst_f32 highpass_triceps; +float highpass_triceps_const[] = {B1, B2, B3, -A2, -A3}; +float highpass_triceps_states[4]; -//Rectifier -float rb; -float rbr; +arm_biquad_casd_df1_inst_f32 notch_triceps; +float notch_triceps_const[] = {D1, D2, D3, -C2, -C3}; +float notch_triceps_states[4]; -//Low pass filter -float xblp; -float xblp1 = 0; -float xblp2 = 0; -float xblp3 = 0; -float xblp4 = 0; -float xblp5 = 0; -float yblp; -float yblp1 = 0; -float yblp2 = 0; -float yblp3 = 0; -float yblp4 = 0; -float yblp5 = 0; +arm_biquad_casd_df1_inst_f32 lowpass_triceps; +float lowpass_triceps_const[] = {F1, F2, F3, -E2, -E3}; +float lowpass_triceps_states[4]; -//Gefilterde biceps EMG -float xbf; -float ybf; +float xtf; +float xtfmax = 0; + +float xbt; +float xtt; volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true @@ -213,53 +186,32 @@ void filter_biceps() { - //High pass - xbhp = xbo; //Input in filter - ybhp = -A2*ybhp1 - A3*ybhp2 - A4*ybhp3 - A5*ybhp4 + B1*xbhp + B2*xbhp1 + B3*xbhp2 + B4*xbhp3 + B5*xbhp4; //Filterformule in z-domein - - //Waarden voor de volgende ronde benoemen - xbhp4 = xbhp3; - xbhp3 = xbhp2; - xbhp2 = xbhp1; - xbhp1 = xbhp; - - ybhp4 = ybhp3; - ybhp3 = ybhp2; - ybhp2 = ybhp1; - ybhp1 = ybhp; - 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 - - //Waarden voor de volgende ronde benoemen - xbn2 = xbn1; - xbn1 = xbn; + arm_biquad_cascade_df1_f32(&highpass_biceps, &xb, &xbf, 1); + + arm_biquad_cascade_df1_f32(¬ch_biceps, &xbf, &xbf, 1); + + xbf = fabs(xbf); + + arm_biquad_cascade_df1_f32(&lowpass_biceps, &xbf, &xbf, 1); - ybn2 = ybn1; - ybn1 = 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 - - 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 - - xblp4 = xblp3; - xblp3 = xblp2; - xblp2 = xblp1; - xblp1 = xblp; - - yblp4 = yblp3; - yblp3 = yblp2; - yblp2 = yblp1; - yblp1 = yblp; - xbf = yblp; //De uiteindelijk gefilterde EMG waarde is de output uit het low pass filter + pc.printf("xbf is %f\n\r", xbf); + + } +void filter_triceps() +{ + arm_biquad_cascade_df1_f32(&highpass_triceps, &xt, &xtf, 1); + + arm_biquad_cascade_df1_f32(¬ch_triceps, &xtf, &xtf, 1); + + xtf = fabs(xtf); + + arm_biquad_cascade_df1_f32(&lowpass_triceps, &xtf, &xtf, 1); + + pc.printf("xtf is %f\n\r", xtf); + +} int main() { @@ -281,7 +233,7 @@ case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie 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) { @@ -310,47 +262,137 @@ ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer break; //Stopt acties in deze case } - - case EMG_KALIBRATIE_BICEPS: - { + + case EMG_KALIBRATIE_BICEPS: { pc.printf("EMG__KALIBRATIE_BICEPS\n\r"); - + lcd_r1 = " SPAN IN 5 SEC. "; lcd_r2 = " 2 X BICEPS AAN "; - + pc.printf("span biceps aan\n\r"); + pc.printf("EMG biceps %f\n\r",xb); + + arm_biquad_cascade_df1_init_f32(&highpass_biceps, 1, highpass_biceps_const, highpass_biceps_states); + arm_biquad_cascade_df1_init_f32(¬ch_biceps, 1, notch_biceps_const, notch_biceps_states); + arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1, lowpass_biceps_const, lowpass_biceps_states); + 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 - - xb = EMG_bi.read(); //EMG meten van biceps - - filter_biceps(); + + while(biceps_kalibratie.read() <= 5) { - if(biceps_kalibratie.read() == 1){ + 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 + + filter_biceps(); + + if(int(biceps_kalibratie.read()) == 1) { lcd_r1 = " 4 "; + pc.printf("4"); } - if(biceps_kalibratie.read() == 2){ + if(biceps_kalibratie.read() == 2) { lcd_r1 = " 3 "; + pc.printf("3"); } - if(biceps_kalibratie.read() == 3){ + if(biceps_kalibratie.read() == 3) { lcd_r1 = " 2 "; + pc.printf("2"); } - if(biceps_kalibratie.read() == 4){ + if(biceps_kalibratie.read() == 4) { + lcd_r1 = " 1 "; + pc.printf("1"); + } + + } + if(xbf >= xbfmax) { + xbfmax = xbf; + } + xbt = xbfmax * 0.8; + pc.printf("threshold is %f\n\r", xbt); + state = EMG_KALIBRATIE_TRICEPS; + break; + } + + case EMG_KALIBRATIE_TRICEPS: { + pc.printf("EMG__KALIBRATIE_TRICEPS\n\r"); + + lcd_r1 = " SPAN IN 5 SEC. "; + lcd_r2 = " 2 X TRICEPS AAN"; + pc.printf("span triceps aan\n\r"); + pc.printf("EMG biceps %f\n\r",xt); + + arm_biquad_cascade_df1_init_f32(&highpass_triceps, 1, highpass_triceps_const, highpass_triceps_states); + arm_biquad_cascade_df1_init_f32(¬ch_triceps, 1, notch_triceps_const, notch_triceps_states); + arm_biquad_cascade_df1_init_f32(&lowpass_triceps, 1, lowpass_triceps_const, lowpass_triceps_states); + + ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); + triceps_kalibratie.start(); + + while(triceps_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 + + xt = EMG_tri.read(); //EMG meten van biceps + + filter_triceps(); + + if(triceps_kalibratie.read() == 1) { + lcd_r1 = " 4 "; + pc.printf("4"); + } + if(triceps_kalibratie.read() == 2) { + lcd_r1 = " 3 "; + pc.printf("3"); + } + if(triceps_kalibratie.read() == 3) { + lcd_r1 = " 2 "; + pc.printf("2"); + } + if(triceps_kalibratie.read() == 4) { lcd_r1 = " 1 "; } - + + } + if(xtf >= xtfmax) { + xtfmax = xtf; } - - state = EMG_KALIBRATIE_TRICEPS; + xtt = xtfmax * 0.8; + pc.printf("threshold is %f\n\r", xtt); + state = BEGIN_METEN; + break; } - - 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 BEGIN_METEN: { + lcd_r1 = " BEGIN "; + pc.printf("KEUZE1\n\r"); + + while(state == BEGIN_METEN) { + 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; + + xb = EMG_bi.read(); + filter_biceps(); + xt = EMG_tri.read(); + filter_triceps(); + + if(xb >= xbt) { + state = B; + } + if(xt >= xtt) { + state = T; + } + } + + + + + 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); } - pc.printf("state: %u\n\r",state); } } \ No newline at end of file