Bouke Scheltinga / Mbed 2 deprecated Werk_college_23sept

Werkcollege opgave 23 september BMT K9

Dependencies:   Encoder HIDScope MODSERIAL mbed QEI biquadFilter

main.cpp@45:10ba78d97d75, 2015-10-21 (annotated)

Committer:
sigert
Date:
Wed Oct 21 13:04:09 2015 +0000
Revision:
45:10ba78d97d75
Parent:
43:9dc385093c8e
Child:
46:c88e0d2daf15
added an average of pwm over 512 samples

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bscheltinga 0:fe3896c6eeb0 1 #include "mbed.h"
bscheltinga 12:0a079e86348e 2 #include "HIDScope.h"
bscheltinga 0:fe3896c6eeb0 3 #include "MODSERIAL.h"
bscheltinga 13:04e10692e239 4 #include "biquadFilter.h" //Filter direct form II
sigert 42:b5186a54d839 5 #include "QEI.h"
bscheltinga 0:fe3896c6eeb0 6
bscheltinga 26:1090acf98efc 7 // [DEFINE INPUTS] //
sigert 42:b5186a54d839 8 DigitalOut debug_led_red (LED_RED); // Debug LED
sigert 37:6c04c15d9bbe 9 DigitalOut debug_led_green(LED_GREEN); // Debug LED
sigert 42:b5186a54d839 10 DigitalOut debug_led_blue (LED_BLUE); // Debug LED
ThomasBNL 36:f29a36683b1a 11
sigert 37:6c04c15d9bbe 12 const double off=1; // Led off
sigert 37:6c04c15d9bbe 13 const double on=0; // Led on
bscheltinga 21:594915ba2bf9 14
sigert 42:b5186a54d839 15 // MOTORS
sigert 42:b5186a54d839 16 QEI motor_turn(D12,D13,NC,32); QEI motor_strike(D9,D10,NC,32); // TURN - STRIKE : Encoder for motor
sigert 42:b5186a54d839 17 PwmOut pwm_motor_turn(D5); PwmOut pwm_motor_strike(D6); // TURN - STRIKE : Pwm for motor
sigert 42:b5186a54d839 18 DigitalOut motordirection_turn(D4); DigitalOut motordirection_strike(D7); // TURN - STRIKE : Direction of the motor
sigert 42:b5186a54d839 19
sigert 42:b5186a54d839 20
bscheltinga 26:1090acf98efc 21 // [DEFINE CONSTANTS] //
ThomasBNL 27:85e5d36bb6c5 22 HIDScope scope(3); // Aantal HIDScope kanalen
bscheltinga 18:68067ffd169e 23 MODSERIAL pc(USBTX,USBRX);
bscheltinga 20:d5f5c60adc43 24 Ticker control_tick;
ThomasBNL 27:85e5d36bb6c5 25 Ticker T1;
bscheltinga 15:7870f7912904 26
bscheltinga 26:1090acf98efc 27 // [BIQUAD FILTERS] //
ThomasBNL 27:85e5d36bb6c5 28 int Fs = 512; // sampling frequency
ThomasBNL 36:f29a36683b1a 29 const double low_b0 = 0.05892937945281792;
ThomasBNL 36:f29a36683b1a 30 const double low_b1 = 0.11785875890563584;
ThomasBNL 36:f29a36683b1a 31 const double low_b2 = 0.05892937945281792;
ThomasBNL 36:f29a36683b1a 32 const double low_a1 = -1.205716572226748;
sigert 37:6c04c15d9bbe 33 const double low_a2 = 0.44143409003801976; //Notch 1 LOW: VIA online biquad calculator Lowpass 520-48-0.7071-6
bscheltinga 15:7870f7912904 34
ThomasBNL 36:f29a36683b1a 35 const double high_b0 = 0.6389437261127494;
ThomasBNL 36:f29a36683b1a 36 const double high_b1 = -1.2778874522254988;
ThomasBNL 36:f29a36683b1a 37 const double high_b2 = 0.6389437261127494;
ThomasBNL 36:f29a36683b1a 38 const double high_a1 = -1.1429772843080923;
ThomasBNL 39:9fa091753592 39 const double high_a2 = 0.41279762014290533; // NOTCH 2 HIGH: VIA online biquad calculator Highpass 520-52-0.7071-6
bscheltinga 22:14abcfdd1554 40
ThomasBNL 39:9fa091753592 41 const double high_b0_HP = 0.84855234544818812;
ThomasBNL 39:9fa091753592 42 const double high_b1_HP = -1.6970469089637623;
ThomasBNL 39:9fa091753592 43 const double high_b2_HP = 0.8485234544818812;
ThomasBNL 39:9fa091753592 44 const double high_a1_HP = -1.6564788473046559;
ThomasBNL 39:9fa091753592 45 const double high_a2_HP = 0.7376149706228688; // HIGHPASS : NOG OPZOEKEN!! : >25Hz? sample rate 512Hz
sigert 37:6c04c15d9bbe 46
ThomasBNL 39:9fa091753592 47 const double low_b0_LP = 0.0013067079602315681;
ThomasBNL 39:9fa091753592 48 const double low_b1_LP = 0.0026134159204631363;
ThomasBNL 39:9fa091753592 49 const double low_b2_LP = 0.0013067079602315681;
ThomasBNL 39:9fa091753592 50 const double low_a1_LP = -1.9238184798980429;
ThomasBNL 39:9fa091753592 51 const double low_a2_LP = 0.9290453117389691; // LOWPASS : NOG OPZOEKEN!! <5-10 Hz? sample rate 512Hz
sigert 37:6c04c15d9bbe 52 //
ThomasBNL 36:f29a36683b1a 53 //Left bicep
sigert 37:6c04c15d9bbe 54 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)
sigert 37:6c04c15d9bbe 55 biquadFilter notchL1(high_a1, high_a2, high_b0, high_b1, high_b2);// moeten nog waardes voor gemaakt worden (>52Hz doorlaten)
ThomasBNL 36:f29a36683b1a 56 biquadFilter notchL2(low_a1, low_a2, low_b0, low_b1, low_b2);// moeten nog waardes voor gemaakt worden (<48Hz doorlaten)
sigert 37:6c04c15d9bbe 57 biquadFilter lowpassfilter_1(low_a1_LP, low_a2_LP, low_b0_LP, low_b1_LP, low_b2_LP);
bscheltinga 0:fe3896c6eeb0 58
ThomasBNL 36:f29a36683b1a 59 // Right bicep
sigert 37:6c04c15d9bbe 60 biquadFilter highpassfilter_2(high_a1_HP, high_a2_HP, high_b0_HP, high_b1_HP, high_b2_HP);// moeten nog waardes voor gemaakt worden?
sigert 37:6c04c15d9bbe 61 biquadFilter notchR1(high_a1, high_a2, high_b0, high_b1, high_b2); // moeten nog waardes voor gemaakt worden
ThomasBNL 36:f29a36683b1a 62 biquadFilter notchR2(low_a1, low_a2, low_b0, low_b1, low_b2); // moeten nog waardes voor gemaakt worden
sigert 37:6c04c15d9bbe 63 biquadFilter lowpassfilter_2(low_a1_LP, low_a2_LP, low_b0_LP, low_b1_LP, low_b2_LP);// moeten nog waardes voor gemaakt worden?
bscheltinga 20:d5f5c60adc43 64
ThomasBNL 27:85e5d36bb6c5 65
ThomasBNL 36:f29a36683b1a 66 AnalogIn input1(A0); // EMG: Two AnalogIn EMG inputs
ThomasBNL 36:f29a36683b1a 67 AnalogIn input2(A1); // EMG: Two AnalogIn EMG inputs
ThomasBNL 36:f29a36683b1a 68
ThomasBNL 36:f29a36683b1a 69 // __________________________
ThomasBNL 36:f29a36683b1a 70 // : [EMG variables] :
ThomasBNL 36:f29a36683b1a 71 // :__________________________:
ThomasBNL 36:f29a36683b1a 72 //
ThomasBNL 36:f29a36683b1a 73
ThomasBNL 36:f29a36683b1a 74 volatile bool control_go = false; // EMG:
ThomasBNL 36:f29a36683b1a 75 volatile bool sample = false;
ThomasBNL 36:f29a36683b1a 76 volatile bool sample_error = false;
ThomasBNL 36:f29a36683b1a 77 volatile bool sample_error_strike = false;
ThomasBNL 36:f29a36683b1a 78
sigert 42:b5186a54d839 79 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);
sigert 42:b5186a54d839 80
ThomasBNL 36:f29a36683b1a 81
ThomasBNL 36:f29a36683b1a 82 double minimum_L; double maximum_L;
ThomasBNL 36:f29a36683b1a 83 double EMG_L_min; double EMG_L_max;
ThomasBNL 36:f29a36683b1a 84 double minimum_R; double maximum_R;
ThomasBNL 36:f29a36683b1a 85 double EMG_R_min; double EMG_R_max;
ThomasBNL 36:f29a36683b1a 86 double EMG_left_Bicep; double EMG_Right_Bicep; // input variables
ThomasBNL 36:f29a36683b1a 87 double EMG_Left_Bicep_filtered_notch_1;double EMG_Right_Bicep_filtered_notch_1;
ThomasBNL 36:f29a36683b1a 88 double EMG_Left_Bicep_filtered_notch_2;double EMG_Right_Bicep_filtered_notch_2;
ThomasBNL 36:f29a36683b1a 89 double EMG_Left_Bicep_filtered; double EMG_Right_Bicep_filtered; // output variables
ThomasBNL 36:f29a36683b1a 90
ThomasBNL 36:f29a36683b1a 91
ThomasBNL 36:f29a36683b1a 92 double n=0; double c=0; double k=0; double p=0; double er=0;
ThomasBNL 36:f29a36683b1a 93
ThomasBNL 36:f29a36683b1a 94 // FUNCTIONS
ThomasBNL 36:f29a36683b1a 95 void ControlGo();
ThomasBNL 36:f29a36683b1a 96 void take_sample();
ThomasBNL 36:f29a36683b1a 97 void Filter();
ThomasBNL 36:f29a36683b1a 98 void sample_filter();
ThomasBNL 36:f29a36683b1a 99 void countdown_from_5();
ThomasBNL 36:f29a36683b1a 100 void calibration();
sigert 37:6c04c15d9bbe 101 void red();void blue();void green();void white();void yellow();void cyan();void purple(); void black();
sigert 37:6c04c15d9bbe 102
sigert 42:b5186a54d839 103 double RS_0,RS_1,RS_2,RS_3,RS_4,RS_5,RS_6,RS_7,RS_8,RS_9,RS_10,
sigert 42:b5186a54d839 104 RS_11,RS_12,RS_13,RS_14,RS_15,RS_16,RS_17,RS_18,RS_19,RS_20,
sigert 42:b5186a54d839 105 RS_21,RS_22,RS_23,RS_24,RS_25,RS_26,RS_27,RS_28,RS_29,RS_30,RS_31;
sigert 42:b5186a54d839 106
sigert 42:b5186a54d839 107 double LS_0,LS_1,LS_2,LS_3,LS_4,LS_5,LS_6,LS_7,LS_8,LS_9,LS_10,
sigert 42:b5186a54d839 108 LS_11,LS_12,LS_13,LS_14,LS_15,LS_16,LS_17,LS_18,LS_19,LS_20,
sigert 42:b5186a54d839 109 LS_21,LS_22,LS_23,LS_24,LS_25,LS_26,LS_27,LS_28,LS_29,LS_30,LS_31;
sigert 42:b5186a54d839 110
sigert 42:b5186a54d839 111 double moving_average_right, moving_average_left;
sigert 42:b5186a54d839 112
sigert 42:b5186a54d839 113 void keep_in_range (double * in, double min, double max);
sigert 42:b5186a54d839 114
sigert 42:b5186a54d839 115 double pwm_strike;
sigert 45:10ba78d97d75 116 double f; double pwm_average; double smp;
ThomasBNL 36:f29a36683b1a 117 //==========================//
ThomasBNL 27:85e5d36bb6c5 118 // [MAIN FUNCTION] //
ThomasBNL 36:f29a36683b1a 119 //==========================//
ThomasBNL 27:85e5d36bb6c5 120 int main()
ThomasBNL 27:85e5d36bb6c5 121 {
ThomasBNL 27:85e5d36bb6c5 122 control_tick.attach(&ControlGo, (float)1/Fs);
sigert 38:be38b9318849 123 pc.baud(115200);
sigert 42:b5186a54d839 124 //double n=0;
sigert 42:b5186a54d839 125 calibration(); // calibreer min en max positie
sigert 45:10ba78d97d75 126 f=1;
sigert 45:10ba78d97d75 127 smp=0;
ThomasBNL 27:85e5d36bb6c5 128 while(1)
ThomasBNL 27:85e5d36bb6c5 129 {
sigert 42:b5186a54d839 130 // if(control_go)
sigert 42:b5186a54d839 131 // {
sigert 42:b5186a54d839 132 // //n++;
sigert 42:b5186a54d839 133 // //pc.printf("n %f",n);
sigert 42:b5186a54d839 134 // sample_filter();
sigert 42:b5186a54d839 135 // scope.set(0,EMG_left_Bicep); //left bicep unfiltered
sigert 42:b5186a54d839 136 // scope.set(1,EMG_Left_Bicep_filtered); //Filtered signal
sigert 42:b5186a54d839 137 // scope.set(2,moving_average_left); //
sigert 42:b5186a54d839 138 // scope.send();
sigert 42:b5186a54d839 139 // control_go = 0;
sigert 42:b5186a54d839 140 // }
sigert 42:b5186a54d839 141 yellow();
ThomasBNL 27:85e5d36bb6c5 142 if(control_go)
sigert 45:10ba78d97d75 143 {
sigert 45:10ba78d97d75 144 control_go=0;
sigert 42:b5186a54d839 145 sample_filter();
sigert 42:b5186a54d839 146 double signal_above_threshold=(moving_average_left); //(moving_average_right-Threshold_Bicep_Right_1)+
sigert 42:b5186a54d839 147 double max_signal=(EMG_L_max); //(EMG_R_max-Threshold_Bicep_Right_1)+
sigert 42:b5186a54d839 148 pwm_strike=signal_above_threshold/max_signal;
sigert 43:9dc385093c8e 149 keep_in_range(&pwm_strike, 0,1);
sigert 42:b5186a54d839 150 pc.printf("Pwm=%f \n\r", pwm_strike);
sigert 43:9dc385093c8e 151 pwm_strike=pwm_strike*pwm_strike;
sigert 45:10ba78d97d75 152
sigert 45:10ba78d97d75 153 pwm_average=pwm_strike+pwm_average/f;
sigert 45:10ba78d97d75 154 f++;
sigert 45:10ba78d97d75 155 smp++;
sigert 45:10ba78d97d75 156 if(smp>512)
sigert 45:10ba78d97d75 157 {
sigert 45:10ba78d97d75 158 pwm_motor_strike=fabs(pwm_strike);
sigert 45:10ba78d97d75 159 green();
sigert 45:10ba78d97d75 160 wait(5);
sigert 45:10ba78d97d75 161 pwm_motor_strike=0;
sigert 45:10ba78d97d75 162 f=1;
sigert 45:10ba78d97d75 163 smp=0;}
sigert 45:10ba78d97d75 164 }
ThomasBNL 27:85e5d36bb6c5 165 } // while end
ThomasBNL 27:85e5d36bb6c5 166 } // main end
ThomasBNL 36:f29a36683b1a 167
ThomasBNL 36:f29a36683b1a 168 // --------------------------------------------------------------------------------------------------------
ThomasBNL 36:f29a36683b1a 169 // [FUNCTIONS] //
ThomasBNL 36:f29a36683b1a 170 void ControlGo() //Control flag
ThomasBNL 36:f29a36683b1a 171 {
ThomasBNL 36:f29a36683b1a 172 control_go = true;
ThomasBNL 36:f29a36683b1a 173 }
ThomasBNL 36:f29a36683b1a 174
ThomasBNL 36:f29a36683b1a 175 void sample_filter()
ThomasBNL 36:f29a36683b1a 176 {
ThomasBNL 36:f29a36683b1a 177 Filter();
ThomasBNL 36:f29a36683b1a 178 take_sample();
ThomasBNL 36:f29a36683b1a 179 if(sample)
ThomasBNL 36:f29a36683b1a 180 {
ThomasBNL 36:f29a36683b1a 181 sample=false;
sigert 42:b5186a54d839 182 LS_0 = EMG_Left_Bicep_filtered; RS_0 = EMG_Right_Bicep_filtered;
sigert 42:b5186a54d839 183
sigert 42:b5186a54d839 184 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;
sigert 42:b5186a54d839 185 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;
sigert 42:b5186a54d839 186 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;
ThomasBNL 36:f29a36683b1a 187
sigert 42:b5186a54d839 188 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;
sigert 42:b5186a54d839 189 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;
sigert 42:b5186a54d839 190 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;
sigert 42:b5186a54d839 191
ThomasBNL 36:f29a36683b1a 192 }
sigert 42:b5186a54d839 193 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;
sigert 42:b5186a54d839 194 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;
ThomasBNL 36:f29a36683b1a 195 n++;
ThomasBNL 36:f29a36683b1a 196 }
ThomasBNL 36:f29a36683b1a 197
ThomasBNL 36:f29a36683b1a 198 void take_sample() // Take a sample every 25th sample
ThomasBNL 36:f29a36683b1a 199 {
sigert 42:b5186a54d839 200 if(n==8)
ThomasBNL 36:f29a36683b1a 201 {
ThomasBNL 36:f29a36683b1a 202 sample = true; n=0;
ThomasBNL 36:f29a36683b1a 203 }
ThomasBNL 36:f29a36683b1a 204
ThomasBNL 36:f29a36683b1a 205 if(er==5)
ThomasBNL 36:f29a36683b1a 206 {
ThomasBNL 36:f29a36683b1a 207 sample_error = true; er=0;
ThomasBNL 36:f29a36683b1a 208 }
ThomasBNL 36:f29a36683b1a 209
ThomasBNL 36:f29a36683b1a 210 sample_error_strike = true;
ThomasBNL 36:f29a36683b1a 211 }
ThomasBNL 36:f29a36683b1a 212
ThomasBNL 36:f29a36683b1a 213 // [FILTER FUNCTIONS] //
ThomasBNL 36:f29a36683b1a 214 // [EMG] //
ThomasBNL 36:f29a36683b1a 215
ThomasBNL 36:f29a36683b1a 216 void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output)
ThomasBNL 36:f29a36683b1a 217 {
ThomasBNL 36:f29a36683b1a 218 EMG_left_Bicep = input1; EMG_Right_Bicep = input2;
ThomasBNL 36:f29a36683b1a 219
ThomasBNL 36:f29a36683b1a 220 EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep);
ThomasBNL 36:f29a36683b1a 221 EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered);
ThomasBNL 36:f29a36683b1a 222
sigert 42:b5186a54d839 223 //EMG_Left_Bicep_filtered_notch_1 = notchL1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered_notch_1 = notchR1.step(EMG_Right_Bicep_filtered);
sigert 42:b5186a54d839 224 //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);
ThomasBNL 36:f29a36683b1a 225
sigert 42:b5186a54d839 226 //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);
sigert 42:b5186a54d839 227
sigert 42:b5186a54d839 228 EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered);
ThomasBNL 36:f29a36683b1a 229 }
ThomasBNL 36:f29a36683b1a 230
ThomasBNL 36:f29a36683b1a 231 void countdown_from_5() // Countdown from 5 till 0 inside Putty (interface)
ThomasBNL 36:f29a36683b1a 232 {
ThomasBNL 36:f29a36683b1a 233 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");
ThomasBNL 36:f29a36683b1a 234 wait(1); pc.printf("1 Set \n\r"); wait(1); pc.printf("Go \n\r");
ThomasBNL 36:f29a36683b1a 235 }
ThomasBNL 36:f29a36683b1a 236
ThomasBNL 36:f29a36683b1a 237 void calibration()
ThomasBNL 36:f29a36683b1a 238 {
ThomasBNL 36:f29a36683b1a 239
ThomasBNL 36:f29a36683b1a 240 // [MINIMUM VALUE BICEPS CALIBRATION] //
ThomasBNL 36:f29a36683b1a 241
ThomasBNL 36:f29a36683b1a 242 pc.printf("Start minimum calibration in 5 seconds \n\r");
ThomasBNL 36:f29a36683b1a 243 pc.printf("Keep your biceps as relaxed as possible \n\r");
ThomasBNL 36:f29a36683b1a 244
ThomasBNL 36:f29a36683b1a 245 countdown_from_5();
ThomasBNL 36:f29a36683b1a 246 c=0;
ThomasBNL 36:f29a36683b1a 247
ThomasBNL 36:f29a36683b1a 248 while(c<2560) // 512Hz -> 2560 is equal to five seconds
ThomasBNL 36:f29a36683b1a 249 {
ThomasBNL 36:f29a36683b1a 250 Filter(); // Filter EMG signal
ThomasBNL 36:f29a36683b1a 251 minimum_L=EMG_Left_Bicep_filtered+minimum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
ThomasBNL 36:f29a36683b1a 252 minimum_R=EMG_Right_Bicep_filtered+minimum_R;
ThomasBNL 36:f29a36683b1a 253 // scope.set(0,EMG_left_Bicep);
ThomasBNL 36:f29a36683b1a 254 // scope.set(1,EMG_Left_Bicep_filtered);
ThomasBNL 36:f29a36683b1a 255 // scope.set(2,minimum_L);
ThomasBNL 36:f29a36683b1a 256 // scope.send();
ThomasBNL 36:f29a36683b1a 257 c++; // Every sample c is increased by one until the statement c<2560 is false
ThomasBNL 36:f29a36683b1a 258 wait(0.001953125); // wait one sample
ThomasBNL 36:f29a36683b1a 259 }
ThomasBNL 36:f29a36683b1a 260
ThomasBNL 36:f29a36683b1a 261 pc.printf("Finished minimum calibration \n\r");
ThomasBNL 36:f29a36683b1a 262
ThomasBNL 36:f29a36683b1a 263 EMG_L_min=minimum_L/2560; // Divide the summation by the number of measurements (2560 measurements) to get a mean value over 5 seconds
ThomasBNL 36:f29a36683b1a 264 EMG_R_min=minimum_R/2560;
ThomasBNL 36:f29a36683b1a 265
ThomasBNL 36:f29a36683b1a 266 pc.printf("EMG_L_min = %f \n\r EMG_R_min = %f \n\r", EMG_L_min, EMG_R_min);
ThomasBNL 36:f29a36683b1a 267
ThomasBNL 36:f29a36683b1a 268 wait (3); //cooldown
ThomasBNL 36:f29a36683b1a 269
ThomasBNL 36:f29a36683b1a 270
ThomasBNL 36:f29a36683b1a 271 // [MAXIMUM VALUE BICEPS CALIBRATION] //
ThomasBNL 36:f29a36683b1a 272
ThomasBNL 36:f29a36683b1a 273
ThomasBNL 36:f29a36683b1a 274 pc.printf("start maximum calibration in 5 seconds (start contraction at 3) \n\r");
ThomasBNL 36:f29a36683b1a 275
ThomasBNL 36:f29a36683b1a 276 countdown_from_5();
ThomasBNL 36:f29a36683b1a 277 c=0;
ThomasBNL 36:f29a36683b1a 278
ThomasBNL 36:f29a36683b1a 279 while(c<2560) // 512Hz -> 2560 is equal to five seconds
ThomasBNL 36:f29a36683b1a 280 {
ThomasBNL 36:f29a36683b1a 281 Filter(); // Filter EMG signal
ThomasBNL 36:f29a36683b1a 282 maximum_L=EMG_Left_Bicep_filtered+maximum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
ThomasBNL 36:f29a36683b1a 283 maximum_R=EMG_Right_Bicep_filtered+maximum_R;
ThomasBNL 36:f29a36683b1a 284 c++; // Every sample c is increased by one until the statement c<2560 is false
ThomasBNL 36:f29a36683b1a 285 wait(0.001953125);
ThomasBNL 36:f29a36683b1a 286 }
ThomasBNL 36:f29a36683b1a 287
ThomasBNL 36:f29a36683b1a 288 pc.printf("Finished minimum calibration \n\r");
ThomasBNL 36:f29a36683b1a 289
ThomasBNL 36:f29a36683b1a 290 EMG_L_max=maximum_L/2560; // Divide the summation by the number of measurements (2560 measurements) to get a mean value over 5 seconds
ThomasBNL 36:f29a36683b1a 291 EMG_R_max=maximum_R/2560;
ThomasBNL 36:f29a36683b1a 292
ThomasBNL 36:f29a36683b1a 293 pc.printf("EMG_L_max = %f \n\r EMG_R_max = %f \n\r", EMG_L_max, EMG_R_max);
ThomasBNL 36:f29a36683b1a 294
ThomasBNL 36:f29a36683b1a 295 wait (3); //cooldown
ThomasBNL 36:f29a36683b1a 296
ThomasBNL 36:f29a36683b1a 297
ThomasBNL 36:f29a36683b1a 298 // [MAXIMUM VALUE BICEPS CALIBRATION] //
ThomasBNL 36:f29a36683b1a 299 // Calculate threshold percentages //
ThomasBNL 36:f29a36683b1a 300
sigert 42:b5186a54d839 301 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
sigert 42:b5186a54d839 302 Threshold_Bicep_Left_2=((EMG_L_max-EMG_L_min)*0.6); //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
sigert 42:b5186a54d839 303 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
sigert 42:b5186a54d839 304 Threshold_Bicep_Right_2=((EMG_R_max-EMG_R_min)*0.6); //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
ThomasBNL 36:f29a36683b1a 305
ThomasBNL 36:f29a36683b1a 306 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);
ThomasBNL 36:f29a36683b1a 307
ThomasBNL 36:f29a36683b1a 308 }
ThomasBNL 36:f29a36683b1a 309
sigert 37:6c04c15d9bbe 310 void red() { debug_led_red=on; debug_led_blue=off; debug_led_green=off; }
sigert 37:6c04c15d9bbe 311 void blue() { debug_led_red=off; debug_led_blue=on; debug_led_green=off; }
sigert 37:6c04c15d9bbe 312 void green() { debug_led_red=off; debug_led_blue=off; debug_led_green=on; }
sigert 37:6c04c15d9bbe 313 void white() { debug_led_red=on; debug_led_blue=on; debug_led_green=on; }
sigert 37:6c04c15d9bbe 314 void yellow() { debug_led_red=on; debug_led_blue=off; debug_led_green=on; }
sigert 37:6c04c15d9bbe 315 void cyan() { debug_led_red=off; debug_led_blue=on; debug_led_green=on; }
sigert 37:6c04c15d9bbe 316 void purple() { debug_led_red=on; debug_led_blue=on; debug_led_green=off; }
sigert 37:6c04c15d9bbe 317 void black() { debug_led_red=off; debug_led_blue=off; debug_led_green=off; }
ThomasBNL 36:f29a36683b1a 318
sigert 42:b5186a54d839 319 void keep_in_range(double * in, double min, double max) // Put in certain min and max values that the input needs to stay within
sigert 42:b5186a54d839 320 {
sigert 42:b5186a54d839 321 *in > min ? *in < max? : *in = max: *in = min;
sigert 42:b5186a54d839 322 }