![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Volledig besturingssysteem voor de myoelectrische prothese arm van mijn bachelor opdracht
Dependencies: mbed QEI MODSERIAL FastPWM biquadFilter
Diff: main.cpp
- Revision:
- 9:ad4bd1b16eed
- Parent:
- 8:7fd9ac522ea9
- Child:
- 10:193942c3900a
--- a/main.cpp Wed Apr 17 14:57:37 2019 +0000 +++ b/main.cpp Fri Apr 19 09:30:32 2019 +0000 @@ -56,9 +56,9 @@ DigitalOut myservo3(D10); //wijsvinger float Periodlength = 0.02; // de MG996R heeft een PWM periode van 20 ms -float servo_position1; // in percentage van 0 tot 1, 0 is met de klok mee, 1 is tegen de klok in. -float servo_position2; -float servo_position3; +double servo_position1; // in percentage van 0 tot 1, 0 is met de klok mee, 1 is tegen de klok in. +double servo_position2; +double servo_position3; // Vinger posities float Duim_krom = 0.05; @@ -78,6 +78,105 @@ float Input2; int count = 0; +//------------- EMG gloabals -------------// +// Tickers +Ticker sample_ticker; //ticker voor filteren met 1000Hz +Ticker threshold_check_ticker; //ticker voor het checken van de threshold met 1000Hz +Timer timer_calibration; //timer voor EMG Kalibratie +double ts = 0.001; //tijdsstap !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! +double calibration_time = 37; //Kalibratie tijd + +//Input +AnalogIn emg1( A1 ); //Duim +AnalogIn emg2( A2 ); //Bicep +AnalogIn emg3( A3 ); //Dorsaal +AnalogIn emg4( A4 ); //Palmair + +// GLOBALS EMG +//Gefilterde EMG signalen +volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered; + +bool thresholdreach1 = false; +bool thresholdreach2 = false; +bool thresholdreach3 = false; +bool thresholdreach4 = false; + +volatile double temp_highest_emg1 = 0; //Hoogste waarde gevonden tijdens kalibratie +volatile double temp_highest_emg2 = 0; +volatile double temp_highest_emg3 = 0; +volatile double temp_highest_emg4 = 0; + +//Percentage van de hoogste waarde waar de bovenste treshold gezet moet worden +double Duim_p_t = 0.5; +double Bicep_p_t = 0.4; +double Dorsaal_p_t = 0.6; +double Palmair_p_t = 0.5; + +//Percentage van de hoogste waarde waar de onderste treshold gezet moet worden +double Duim_p_tL = 0.5; +double Bicep_p_tL = 0.4; +double Dorsaal_p_tL = 0.5; +double Palmair_p_tL = 0.5; + +// Waarde bovenste treshold waar het signaal overheen moet om de arm te activeren +volatile double threshold1; +volatile double threshold2; +volatile double threshold3; +volatile double threshold4; + +// Waarde onderste treshold waar het signaal onder moet om de arm te deactiveren +volatile double threshold1L; +volatile double threshold2L; +volatile double threshold3L; +volatile double threshold4L; + +// thresholdreads bools +int Duim; +int Bicep; +int Dorsaal; +int Palmair; + +// filters +//EMG1!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! +//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren +BiQuadChain highp1; +BiQuad highp1_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 ); +BiQuad highp1_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 ); +BiQuad notch1_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 ); + +//Lowpass first order cutoff 0.4Hz +BiQuad lowp1( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 ); + +//EMG2!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! +//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren +BiQuadChain highp2; +BiQuad highp2_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 ); +BiQuad highp2_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 ); +BiQuad notch2_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 ); + +//Lowpass first order cutoff 0.4Hz +BiQuad lowp2( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 ); + +//EMG3!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! +//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren +BiQuadChain highp3; +BiQuad highp3_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 ); +BiQuad highp3_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 ); +BiQuad notch3_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 ); + +//Lowpass first order cutoff 0.4Hz +BiQuad lowp3( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 ); + +//EMG4!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! +//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren +BiQuadChain highp4; +BiQuad highp4_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 ); +BiQuad highp4_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 ); +BiQuad notch4_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 ); + +//Lowpass first order cutoff 0.4Hz +BiQuad lowp4( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 ); + //Kinematica double stap1; double stap2; @@ -113,6 +212,128 @@ double Kd2 = 1; double Ts = 0.0005; // Sample time in seconds + +// ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~ +void emgsample() +{ + + // EMG signaal lezen + double emgread1 = emg1.read(); + double emgread2 = emg2.read(); + double emgread3 = emg3.read(); + double emgread4 = emg4.read(); + + // Vierde orde highpass filter + notch filter + double emg1_highpassed = highp1.step(emgread1); + double emg2_highpassed = highp2.step(emgread2); + double emg3_highpassed = highp3.step(emgread3); + double emg4_highpassed = highp4.step(emgread4); + + //Rectificatie + double emg1_abs = abs(emg1_highpassed); + double emg2_abs = abs(emg2_highpassed); + double emg3_abs = abs(emg3_highpassed); + double emg4_abs = abs(emg4_highpassed); + + //All EMG abs into lowpass + emg1_filtered = lowp1.step(emg1_abs); + emg2_filtered = lowp2.step(emg2_abs); + emg3_filtered = lowp3.step(emg3_abs); + emg4_filtered = lowp4.step(emg4_abs); + +} + +//Check of emg_filtered boven de threshold is +void threshold_check() +{ + +// EMG1 Check +if (thresholdreach1 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest + //bovenste threshold check + if(emg1_filtered>threshold1) { + Duim = 1; + thresholdreach1 = true; + + } else { + Duim= 0; + } + } +else{ //Als emg_filtered boven de bovenste threshold is geweest + //onderste threshold check + if(emg1_filtered<threshold1L) { + Duim = 0; + thresholdreach1 = false; + + } else { + Duim= 1; + } + } + +// EMG2 Check +if (thresholdreach2 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest + //bovenste threshold check + if(emg2_filtered>threshold2) { + Bicep = 1; + thresholdreach2 = true; + } else { + Bicep= 0; + } + } +else{ //Als emg_filtered boven de bovenste threshold is geweest + //onderste threshold check + if(emg2_filtered<threshold2L) { + Bicep = 0; + thresholdreach2 = false; + + } else { + Bicep= 1; + } + } + +// EMG3 Check +if (thresholdreach3 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest + //bovenste threshold check + if(emg3_filtered>threshold3) { + Dorsaal = 1; + thresholdreach3 = true; + } else { + Dorsaal= 0; + } + } +else{ //Als emg_filtered boven de bovenste threshold is geweest + //onderste threshold check + if(emg3_filtered<threshold3L) { + Dorsaal = 0; + thresholdreach3 = false; + + } else { + Dorsaal= 1; + } + } + +// EMG4 Check +if (thresholdreach4 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest + //bovenste threshold check + if(emg4_filtered>threshold4) { + Palmair = 1; + thresholdreach4 = true; + } else { + Palmair= 0; + } + } +else{ //Als emg_filtered boven de bovenste threshold is geweest + //onderste threshold check + if(emg4_filtered<threshold4L) { + Palmair = 0; + thresholdreach4 = false; + + } else { + Palmair= 1; + } + } + + } + // Functies Kinematica float Kinematics1(float EMG1) { @@ -345,6 +566,15 @@ pwm1 =(Pot1*2)-1; } +void sample() +{ +pc.printf("Duim Right = %i\r\n", Duim); +pc.printf("Bicep Right = %i\r\n",Bicep); +pc.printf("Dorsaal Left = %i\r\n", Dorsaal); +pc.printf("Palmair Left = %i\r\n", Palmair); + +} + // StateMachine enum states {MOTORS_OFF,CALIBRATION,HOMING1,HOMING2,DEMO,MOVEMENT,FREEZE}; @@ -385,12 +615,52 @@ led1 = 0; led2 = 0; led3 = 1; + sample_ticker.attach(&emgsample, 0.001); // Leest het ruwe EMG signaal af met een frequentie van 1000Hz + timer_calibration.reset(); + timer_calibration.start(); + while(timer_calibration<20) { //Duim + if(timer_calibration>0 && timer_calibration<20) { + led1=!led1; + if(emg1_filtered>temp_highest_emg1) { + temp_highest_emg1= emg1_filtered; + pc.printf("Highest value Duim= %f \r\n", temp_highest_emg1); + } - wait(1); + if(emg2_filtered>temp_highest_emg2) { + temp_highest_emg2= emg2_filtered; + pc.printf("Highest value Bicep= %f \r\n", temp_highest_emg2); + } + + if(emg3_filtered>temp_highest_emg3) { + temp_highest_emg3= emg3_filtered; + pc.printf("Highest value Dorsaal= %f \r\n", temp_highest_emg3); + } + + if(emg4_filtered>temp_highest_emg4) { + temp_highest_emg4= emg4_filtered; + pc.printf("Highest value Palmair= %f \r\n", temp_highest_emg4); + } + + } + led1=1; + led2=1; + led3=1; + + } + pc.printf("threshold calculation\r\n"); + threshold1 = temp_highest_emg1*Duim_p_t; + threshold2 = temp_highest_emg2*Bicep_p_t; + threshold3 = temp_highest_emg3*Dorsaal_p_t; + threshold4 = temp_highest_emg4*Palmair_p_t; + + threshold1L = temp_highest_emg1*Duim_p_tL; + threshold2L = temp_highest_emg2*Bicep_p_tL; + threshold3L = temp_highest_emg3*Dorsaal_p_tL; + threshold4L = temp_highest_emg4*Palmair_p_tL; stateChanged = false; } - + sample_ticker.detach(); // State transition logic: automatisch terug naar motors off. currentState = HOMING1;