Werkcollege opgave 23 september BMT K9
Dependencies: Encoder HIDScope MODSERIAL mbed QEI biquadFilter
main.cpp
- Committer:
- ThomasBNL
- Date:
- 2015-10-26
- Revision:
- 51:b40967b1fe5c
- Parent:
- 50:c4c9daf7b74c
- Child:
- 52:45200bbde108
File content as of revision 51:b40967b1fe5c:
#include "mbed.h" #include "HIDScope.h" #include "MODSERIAL.h" #include "biquadFilter.h" //Filter direct form II #include "QEI.h" const double ledon = 1; const double ledoff = 0; DigitalOut ledgreen1(D0); DigitalOut ledgreen2(D1); DigitalOut ledyellow1(D2); DigitalOut ledyellow2(D3); DigitalOut ledred1(D9); DigitalOut ledred2(D10); void leduit() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledoff; } // [DEFINE INPUTS] // DigitalOut debug_led_red (LED_RED); // Debug LED DigitalOut debug_led_green(LED_GREEN); // Debug LED DigitalOut debug_led_blue (LED_BLUE); // Debug LED const double off=1; // Led off const double on=0; // Led on // MOTORS QEI motor_turn(D12,D13,NC,32); QEI motor_strike(D9,D10,NC,32); // TURN - STRIKE : Encoder for motor PwmOut pwm_motor_turn(D5); PwmOut pwm_motor_strike(D6); // TURN - STRIKE : Pwm for motor DigitalOut motordirection_turn(D4); DigitalOut motordirection_strike(D7); // TURN - STRIKE : Direction of the motor // [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; //Notch 1 LOW: VIA online biquad calculator Lowpass 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; // NOTCH 2 HIGH: VIA online biquad calculator Highpass 520-52-0.7071-6 const double high_b0_HP = 0.84855234544818812; const double high_b1_HP = -1.6970469089637623; const double high_b2_HP = 0.8485234544818812; const double high_a1_HP = -1.6564788473046559; const double high_a2_HP = 0.7376149706228688; // HIGHPASS : NOG OPZOEKEN!! : >25Hz? sample rate 512Hz const double low_b0_LP = 0.0013067079602315681; const double low_b1_LP = 0.0026134159204631363; const double low_b2_LP = 0.0013067079602315681; const double low_a1_LP = -1.9238184798980429; const double low_a2_LP = 0.9290453117389691; // LOWPASS : NOG OPZOEKEN!! <5-10 Hz? sample rate 512Hz // //Left bicep biquadFilter highpassfilter_1(high_a1_HP, high_a2_HP, high_b0_HP, high_b1_HP, high_b2_HP);// moeten nog waardes voor gemaakt worden? (>25Hz doorlaten) biquadFilter notchL1(high_a1, high_a2, high_b0, high_b1, high_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_LP, low_a2_LP, low_b0_LP, low_b1_LP, low_b2_LP); // Right bicep biquadFilter highpassfilter_2(high_a1_HP, high_a2_HP, high_b0_HP, high_b1_HP, high_b2_HP);// moeten nog waardes voor gemaakt worden? biquadFilter notchR1(high_a1, high_a2, high_b0, high_b1, high_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_LP, low_a2_LP, low_b0_LP, low_b1_LP, low_b2_LP);// 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 Threshold_Bicep_Left_1, Threshold_Bicep_Left_2, Threshold_Bicep_Right_1, Threshold_Bicep_Right_2; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is); 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(); void red();void blue();void green();void white();void yellow();void cyan();void purple(); void black(); double RS_0,RS_1,RS_2,RS_3,RS_4,RS_5,RS_6,RS_7,RS_8,RS_9,RS_10, RS_11,RS_12,RS_13,RS_14,RS_15,RS_16,RS_17,RS_18,RS_19,RS_20, RS_21,RS_22,RS_23,RS_24,RS_25,RS_26,RS_27,RS_28,RS_29,RS_30,RS_31; double LS_0,LS_1,LS_2,LS_3,LS_4,LS_5,LS_6,LS_7,LS_8,LS_9,LS_10, LS_11,LS_12,LS_13,LS_14,LS_15,LS_16,LS_17,LS_18,LS_19,LS_20, LS_21,LS_22,LS_23,LS_24,LS_25,LS_26,LS_27,LS_28,LS_29,LS_30,LS_31; double moving_average_right, moving_average_left; void keep_in_range (double * in, double min, double max); double pwm_strike; double f; double pwm_average; double smp; //==========================// // [MAIN FUNCTION] // //==========================// int main() { control_tick.attach(&ControlGo, (float)1/Fs); pc.baud(115200); //double n=0; calibration(); // calibreer min en max positie f=1; smp=0; while(1) { // if(control_go) // { // //n++; // //pc.printf("n %f",n); // 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; // } yellow(); if(control_go) { control_go=0; 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(); double signal_above_threshold=(moving_average_left); //(moving_average_right-Threshold_Bicep_Right_1)+ double max_signal=(EMG_L_max); //(EMG_R_max-Threshold_Bicep_Right_1)+ pwm_strike=signal_above_threshold/max_signal; keep_in_range(&pwm_strike, 0,1); pwm_strike=pwm_strike*pwm_strike; pwm_average=pwm_strike+pwm_average/f; f++; smp++; pc.printf("Pwm=%f \n\r", pwm_average); double speed=fabs(pwm_average); if (pwm_average < 0.1) { leduit(); } if (pwm_average > 0.1) { ledgreen1 = ledon; } if (pwm_average > 0.2) { ledgreen2 = ledon; } if (pwm_average > 0.3) { ledyellow1 = ledon; } if (pwm_average > 0.5) { ledyellow2 = ledon; } if (pwm_average > 0.7) { ledred1 = ledon; } if (pwm_average > 0.9) { ledred2 = ledon; } if(smp>512) { pwm_motor_strike=speed; pwm_motor_turn=speed; green(); pwm_motor_strike=0; wait(3); f=1; smp=0;} } } // while end } // main end // -------------------------------------------------------------------------------------------------------- // [FUNCTIONS] // void ControlGo() //Control flag { control_go = true; } void sample_filter() { Filter(); take_sample(); if(sample) { sample=false; LS_0 = EMG_Left_Bicep_filtered; RS_0 = EMG_Right_Bicep_filtered; LS_30=LS_29; LS_29=LS_28;LS_28=LS_27; LS_27=LS_26; LS_26=LS_25; LS_25=LS_24; LS_24=LS_23; LS_23=LS_22; LS_22=LS_21; LS_20=LS_19; LS_19=LS_18; LS_18=LS_17;LS_17=LS_16; LS_16=LS_15; LS_15=LS_14; LS_14=LS_13; LS_13=LS_12; LS_12=LS_11; LS_11=LS_10; LS_10=LS_9; LS_9=LS_8; LS_8=LS_7;LS_7=LS_6; LS_6=LS_5; LS_5=LS_4; LS_4=LS_3; LS_3=LS_2; LS_2=LS_1; LS_1=LS_0; RS_30=RS_29; RS_29=RS_28;RS_28=RS_27; RS_27=RS_26; RS_26=RS_25; RS_25=RS_24; RS_24=RS_23; RS_23=RS_22; RS_22=RS_21; RS_20=RS_19; RS_19=RS_18; RS_18=RS_17;RS_17=RS_16; RS_16=RS_15; RS_15=RS_14; RS_14=RS_13; RS_13=RS_12; RS_12=RS_11; RS_11=RS_10; RS_10=RS_9; RS_9=RS_8; RS_8=RS_7;RS_7=RS_6; RS_6=RS_5; RS_5=RS_4; RS_4=RS_3; RS_3=RS_2; RS_2=RS_1; RS_1=RS_0; } moving_average_left= (LS_30+LS_29+LS_28+LS_27+LS_26+LS_25+LS_24+LS_23+LS_22+LS_21+LS_20+LS_19+LS_18+LS_17+LS_16+LS_15+LS_14+LS_13+LS_12+LS_11+LS_10+LS_9+LS_8+LS_7+LS_6+LS_5+LS_4+LS_3+LS_2+LS_1+LS_0)/31; moving_average_right= (RS_30+RS_29+RS_28+RS_27+RS_26+RS_25+RS_24+RS_23+RS_22+RS_21+RS_20+RS_19+RS_18+RS_17+RS_16+RS_15+RS_14+RS_13+RS_12+RS_11+RS_10+RS_9+RS_8+RS_7+RS_6+RS_5+RS_4+RS_3+RS_2+RS_1+RS_0)/31; n++; } void take_sample() // Take a sample every 25th sample { if(n==8) { 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) { //ZONDER NOTCH 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 = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered); // 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_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); // // // EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered); // // EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered); } 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 // Threshold_Bicep_Left_1=((EMG_L_max-EMG_L_min)*0.2); //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // LEFT Threshold_Bicep_Left_2=((EMG_L_max-EMG_L_min)*0.6); //(waarde waarop het gemeten EMG signaal 60% van max het maximale is); Threshold_Bicep_Right_1=((EMG_R_max-EMG_R_min)*0.2); //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // RIGHT Threshold_Bicep_Right_2=((EMG_R_max-EMG_R_min)*0.6); //(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); } void red() { debug_led_red=on; debug_led_blue=off; debug_led_green=off; } void blue() { debug_led_red=off; debug_led_blue=on; debug_led_green=off; } void green() { debug_led_red=off; debug_led_blue=off; debug_led_green=on; } void white() { debug_led_red=on; debug_led_blue=on; debug_led_green=on; } void yellow() { debug_led_red=on; debug_led_blue=off; debug_led_green=on; } void cyan() { debug_led_red=off; debug_led_blue=on; debug_led_green=on; } void purple() { debug_led_red=on; debug_led_blue=on; debug_led_green=off; } void black() { debug_led_red=off; debug_led_blue=off; debug_led_green=off; } void keep_in_range(double * in, double min, double max) // Put in certain min and max values that the input needs to stay within { *in > min ? *in < max? : *in = max: *in = min; }