Werkcollege opgave 23 september BMT K9
Dependencies: Encoder HIDScope MODSERIAL mbed QEI biquadFilter
main.cpp
- Committer:
- ThomasBNL
- Date:
- 2015-10-19
- Revision:
- 36:f29a36683b1a
- Parent:
- 32:97cf6cb8d054
- Child:
- 37:6c04c15d9bbe
File content as of revision 36:f29a36683b1a:
#include "mbed.h" #include "HIDScope.h" #include "MODSERIAL.h" #include "biquadFilter.h" //Filter direct form II // [DEFINE INPUTS] // // [DEFINE CONSTANTS] // HIDScope scope(3); // Aantal HIDScope kanalen MODSERIAL pc(USBTX,USBRX); Ticker control_tick; Ticker T1; // [BIQUAD FILTERS] // int Fs = 512; // sampling frequency const double low_b0 = 0.05892937945281792; const double low_b1 = 0.11785875890563584; const double low_b2 = 0.05892937945281792; const double low_a1 = -1.205716572226748; const double low_a2 = 0.44143409003801976; // VIA online biquad calculator Lowpas 520-48-0.7071-6 const double high_b0 = 0.6389437261127494; const double high_b1 = -1.2778874522254988; const double high_b2 = 0.6389437261127494; const double high_a1 = -1.1429772843080923; const double high_a2 = 0.41279762014290533; // VIA online biquad calculator Highpas 520-52-0.7071-6 //Left bicep biquadFilter highpassfilter_1(high_a1, high_a2, high_b0, high_b1, high_b2);// moeten nog waardes voor gemaakt worden? (>25Hz doorlaten) biquadFilter notchL1(low_a1, low_a2, low_b0, low_b1, low_b2);// moeten nog waardes voor gemaakt worden (>52Hz doorlaten) biquadFilter notchL2(low_a1, low_a2, low_b0, low_b1, low_b2);// moeten nog waardes voor gemaakt worden (<48Hz doorlaten) biquadFilter lowpassfilter_1(low_a1, low_a2, low_b0, low_b1, low_b2);// moeten nog waardes voor gemaakt worden? (<5-10 Hz ???) // (<200 Hz doorlaten) => wat is het verschil wat is bruikbaar // Right bicep biquadFilter highpassfilter_2(high_a1, high_a2, high_b0, high_b1, high_b2);// moeten nog waardes voor gemaakt worden? biquadFilter notchR1(low_a1, low_a2, low_b0, low_b1, low_b2); // moeten nog waardes voor gemaakt worden biquadFilter notchR2(low_a1, low_a2, low_b0, low_b1, low_b2); // moeten nog waardes voor gemaakt worden biquadFilter lowpassfilter_2(low_a1, low_a2, low_b0, low_b1, low_b2);// moeten nog waardes voor gemaakt worden? AnalogIn input1(A0); // EMG: Two AnalogIn EMG inputs AnalogIn input2(A1); // EMG: Two AnalogIn EMG inputs // __________________________ // : [EMG variables] : // :__________________________: // volatile bool control_go = false; // EMG: volatile bool sample = false; volatile bool sample_error = false; volatile bool sample_error_strike = false; double Sample_EMG_L_1, Sample_EMG_L_2, Sample_EMG_L_3, Sample_EMG_L_4, Sample_EMG_L_5, Sample_EMG_L_6, Sample_EMG_L_7, Sample_EMG_L_8, Sample_EMG_L_9, Sample_EMG_L_10, moving_average_left; double Sample_EMG_R_1, Sample_EMG_R_2, Sample_EMG_R_3, Sample_EMG_R_4, Sample_EMG_R_5, Sample_EMG_R_6, Sample_EMG_R_7, Sample_EMG_R_8, Sample_EMG_R_9, Sample_EMG_R_10, moving_average_right; double minimum_L; double maximum_L; double EMG_L_min; double EMG_L_max; double minimum_R; double maximum_R; double EMG_R_min; double EMG_R_max; double EMG_left_Bicep; double EMG_Right_Bicep; // input variables double EMG_Left_Bicep_filtered_notch_1;double EMG_Right_Bicep_filtered_notch_1; double EMG_Left_Bicep_filtered_notch_2;double EMG_Right_Bicep_filtered_notch_2; double EMG_Left_Bicep_filtered; double EMG_Right_Bicep_filtered; // output variables double n=0; double c=0; double k=0; double p=0; double er=0; // FUNCTIONS void ControlGo(); void take_sample(); void Filter(); void sample_filter(); void countdown_from_5(); void calibration(); //==========================// // [MAIN FUNCTION] // //==========================// int main() { control_tick.attach(&ControlGo, (float)1/Fs); pc.baud(9600); // calibration(); // calibreer min en max positie while(1) { if(control_go) { sample_filter(); scope.set(0,EMG_left_Bicep); //left bicep unfiltered scope.set(1,EMG_Left_Bicep_filtered); //Filtered signal scope.set(2,moving_average_left); // scope.send(); control_go = 0; } } // while end } // main end // -------------------------------------------------------------------------------------------------------- // [FUNCTIONS] // void ControlGo() //Control flag { control_go = true; } void sample_filter() { Filter(); take_sample(); if(sample) { sample=false; Sample_EMG_L_1 = EMG_Left_Bicep_filtered; Sample_EMG_R_1 = EMG_Right_Bicep_filtered; Sample_EMG_L_10= Sample_EMG_L_9; Sample_EMG_R_10= Sample_EMG_R_9; Sample_EMG_L_9 = Sample_EMG_L_8; Sample_EMG_R_9 = Sample_EMG_R_8; Sample_EMG_L_8 = Sample_EMG_L_7; Sample_EMG_R_8 = Sample_EMG_R_7; Sample_EMG_L_7 = Sample_EMG_L_6; Sample_EMG_R_7 = Sample_EMG_R_6; Sample_EMG_L_6 = Sample_EMG_L_5; Sample_EMG_R_6 = Sample_EMG_R_5; Sample_EMG_L_5 = Sample_EMG_L_4; Sample_EMG_R_5 = Sample_EMG_R_4; Sample_EMG_L_4 = Sample_EMG_L_3; Sample_EMG_R_4 = Sample_EMG_R_3; Sample_EMG_L_3 = Sample_EMG_L_2; Sample_EMG_R_3 = Sample_EMG_R_2; Sample_EMG_L_2 = Sample_EMG_L_1; Sample_EMG_R_2 = Sample_EMG_R_1; } moving_average_left=Sample_EMG_L_1*0.1+Sample_EMG_L_2*0.1+Sample_EMG_L_3*0.1+Sample_EMG_L_4*0.1+Sample_EMG_L_5*0.1+Sample_EMG_L_6*0.1+Sample_EMG_L_7*0.1+Sample_EMG_L_8*0.1+Sample_EMG_L_9*0.1+Sample_EMG_L_10*0.1; moving_average_right=Sample_EMG_R_1*0.1+Sample_EMG_R_2*0.1+Sample_EMG_R_3*0.1+Sample_EMG_R_4*0.1+Sample_EMG_R_5*0.1+Sample_EMG_R_6*0.1+Sample_EMG_R_7*0.1+Sample_EMG_R_8*0.1+Sample_EMG_R_9*0.1+Sample_EMG_R_10*0.1; n++; } void take_sample() // Take a sample every 25th sample { if(n==25) { sample = true; n=0; } if(er==5) { sample_error = true; er=0; } sample_error_strike = true; } // [FILTER FUNCTIONS] // // [EMG] // void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output) { EMG_left_Bicep = input1; EMG_Right_Bicep = input2; EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep); EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered); EMG_Left_Bicep_filtered_notch_1 = notchL1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered_notch_1 = notchR1.step(EMG_Right_Bicep_filtered); EMG_Left_Bicep_filtered_notch_2 = notchL2.step(EMG_Left_Bicep_filtered_notch_1); EMG_Right_Bicep_filtered_notch_2 = notchR2.step(EMG_Right_Bicep_filtered_notch_1); EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered_notch_2); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered_notch_2); } void countdown_from_5() // Countdown from 5 till 0 inside Putty (interface) { wait(1); pc.printf("5 \n\r"); wait(1); pc.printf("4 \n\r"); wait(1); pc.printf("3 \n\r"); wait(1); pc.printf("2 Ready \n\r"); wait(1); pc.printf("1 Set \n\r"); wait(1); pc.printf("Go \n\r"); } void calibration() { // [MINIMUM VALUE BICEPS CALIBRATION] // pc.printf("Start minimum calibration in 5 seconds \n\r"); pc.printf("Keep your biceps as relaxed as possible \n\r"); countdown_from_5(); c=0; while(c<2560) // 512Hz -> 2560 is equal to five seconds { Filter(); // Filter EMG signal minimum_L=EMG_Left_Bicep_filtered+minimum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value minimum_R=EMG_Right_Bicep_filtered+minimum_R; // scope.set(0,EMG_left_Bicep); // scope.set(1,EMG_Left_Bicep_filtered); // scope.set(2,minimum_L); // scope.send(); c++; // Every sample c is increased by one until the statement c<2560 is false wait(0.001953125); // wait one sample } pc.printf("Finished minimum calibration \n\r"); EMG_L_min=minimum_L/2560; // Divide the summation by the number of measurements (2560 measurements) to get a mean value over 5 seconds EMG_R_min=minimum_R/2560; pc.printf("EMG_L_min = %f \n\r EMG_R_min = %f \n\r", EMG_L_min, EMG_R_min); wait (3); //cooldown // [MAXIMUM VALUE BICEPS CALIBRATION] // pc.printf("start maximum calibration in 5 seconds (start contraction at 3) \n\r"); countdown_from_5(); c=0; while(c<2560) // 512Hz -> 2560 is equal to five seconds { Filter(); // Filter EMG signal maximum_L=EMG_Left_Bicep_filtered+maximum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value maximum_R=EMG_Right_Bicep_filtered+maximum_R; c++; // Every sample c is increased by one until the statement c<2560 is false wait(0.001953125); } pc.printf("Finished minimum calibration \n\r"); EMG_L_max=maximum_L/2560; // Divide the summation by the number of measurements (2560 measurements) to get a mean value over 5 seconds EMG_R_max=maximum_R/2560; pc.printf("EMG_L_max = %f \n\r EMG_R_max = %f \n\r", EMG_L_max, EMG_R_max); wait (3); //cooldown // [MAXIMUM VALUE BICEPS CALIBRATION] // // Calculate threshold percentages // const float Threshold_Bicep_Left_1=((EMG_L_max-EMG_L_min)*0.2)+EMG_L_min;; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // LEFT const float Threshold_Bicep_Left_2=((EMG_L_max-EMG_L_min)*0.6)+EMG_L_min; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is); const float Threshold_Bicep_Right_1=((EMG_R_max-EMG_R_min)*0.2)+EMG_R_min; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // RIGHT const float Threshold_Bicep_Right_2=((EMG_R_max-EMG_R_min)*0.6)+EMG_R_min; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is); pc.printf("left 1: %f left 2: %f right 1: %f right 2: %f \n\r", Threshold_Bicep_Left_1, Threshold_Bicep_Left_2, Threshold_Bicep_Right_1, Threshold_Bicep_Right_2); }