Bouke Scheltinga / Mbed 2 deprecated Werk_college_23sept

Werkcollege opgave 23 september BMT K9

Dependencies:   Encoder HIDScope MODSERIAL mbed QEI biquadFilter

main.cpp@42:b5186a54d839, 2015-10-21 (annotated)

Committer:
sigert
Date:
Wed Oct 21 12:43:01 2015 +0000
Revision:
42:b5186a54d839
Parent:
39:9fa091753592
Child:
43:9dc385093c8e
Pwm aansturen met EMG spierkracht

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;
ThomasBNL 36:f29a36683b1a 116
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
ThomasBNL 27:85e5d36bb6c5 126 while(1)
ThomasBNL 27:85e5d36bb6c5 127 {
sigert 42:b5186a54d839 128 // if(control_go)
sigert 42:b5186a54d839 129 // {
sigert 42:b5186a54d839 130 // //n++;
sigert 42:b5186a54d839 131 // //pc.printf("n %f",n);
sigert 42:b5186a54d839 132 // sample_filter();
sigert 42:b5186a54d839 133 // scope.set(0,EMG_left_Bicep); //left bicep unfiltered
sigert 42:b5186a54d839 134 // scope.set(1,EMG_Left_Bicep_filtered); //Filtered signal
sigert 42:b5186a54d839 135 // scope.set(2,moving_average_left); //
sigert 42:b5186a54d839 136 // scope.send();
sigert 42:b5186a54d839 137 // control_go = 0;
sigert 42:b5186a54d839 138 // }
sigert 42:b5186a54d839 139 yellow();
sigert 42:b5186a54d839 140
ThomasBNL 27:85e5d36bb6c5 141 if(control_go)
sigert 42:b5186a54d839 142 {control_go=0;
sigert 42:b5186a54d839 143 sample_filter();
sigert 42:b5186a54d839 144 double signal_above_threshold=(moving_average_left); //(moving_average_right-Threshold_Bicep_Right_1)+
sigert 42:b5186a54d839 145 double max_signal=(EMG_L_max); //(EMG_R_max-Threshold_Bicep_Right_1)+
sigert 42:b5186a54d839 146 pwm_strike=signal_above_threshold/max_signal;
sigert 42:b5186a54d839 147 pc.printf("Pwm=%f \n\r", pwm_strike);
sigert 42:b5186a54d839 148 keep_in_range(&pwm_strike, -1,1);
sigert 42:b5186a54d839 149
sigert 42:b5186a54d839 150 pwm_motor_strike=fabs(pwm_strike);}
ThomasBNL 27:85e5d36bb6c5 151 } // while end
ThomasBNL 27:85e5d36bb6c5 152 } // main end
ThomasBNL 36:f29a36683b1a 153
ThomasBNL 36:f29a36683b1a 154 // --------------------------------------------------------------------------------------------------------
ThomasBNL 36:f29a36683b1a 155 // [FUNCTIONS] //
ThomasBNL 36:f29a36683b1a 156 void ControlGo() //Control flag
ThomasBNL 36:f29a36683b1a 157 {
ThomasBNL 36:f29a36683b1a 158 control_go = true;
ThomasBNL 36:f29a36683b1a 159 }
ThomasBNL 36:f29a36683b1a 160
ThomasBNL 36:f29a36683b1a 161 void sample_filter()
ThomasBNL 36:f29a36683b1a 162 {
ThomasBNL 36:f29a36683b1a 163 Filter();
ThomasBNL 36:f29a36683b1a 164 take_sample();
ThomasBNL 36:f29a36683b1a 165 if(sample)
ThomasBNL 36:f29a36683b1a 166 {
ThomasBNL 36:f29a36683b1a 167 sample=false;
sigert 42:b5186a54d839 168 LS_0 = EMG_Left_Bicep_filtered; RS_0 = EMG_Right_Bicep_filtered;
sigert 42:b5186a54d839 169
sigert 42:b5186a54d839 170 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 171 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 172 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 173
sigert 42:b5186a54d839 174 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 175 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 176 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 177
ThomasBNL 36:f29a36683b1a 178 }
sigert 42:b5186a54d839 179 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 180 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 181 n++;
ThomasBNL 36:f29a36683b1a 182 }
ThomasBNL 36:f29a36683b1a 183
ThomasBNL 36:f29a36683b1a 184 void take_sample() // Take a sample every 25th sample
ThomasBNL 36:f29a36683b1a 185 {
sigert 42:b5186a54d839 186 if(n==8)
ThomasBNL 36:f29a36683b1a 187 {
ThomasBNL 36:f29a36683b1a 188 sample = true; n=0;
ThomasBNL 36:f29a36683b1a 189 }
ThomasBNL 36:f29a36683b1a 190
ThomasBNL 36:f29a36683b1a 191 if(er==5)
ThomasBNL 36:f29a36683b1a 192 {
ThomasBNL 36:f29a36683b1a 193 sample_error = true; er=0;
ThomasBNL 36:f29a36683b1a 194 }
ThomasBNL 36:f29a36683b1a 195
ThomasBNL 36:f29a36683b1a 196 sample_error_strike = true;
ThomasBNL 36:f29a36683b1a 197 }
ThomasBNL 36:f29a36683b1a 198
ThomasBNL 36:f29a36683b1a 199 // [FILTER FUNCTIONS] //
ThomasBNL 36:f29a36683b1a 200 // [EMG] //
ThomasBNL 36:f29a36683b1a 201
ThomasBNL 36:f29a36683b1a 202 void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output)
ThomasBNL 36:f29a36683b1a 203 {
ThomasBNL 36:f29a36683b1a 204 EMG_left_Bicep = input1; EMG_Right_Bicep = input2;
ThomasBNL 36:f29a36683b1a 205
ThomasBNL 36:f29a36683b1a 206 EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep);
ThomasBNL 36:f29a36683b1a 207 EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered);
ThomasBNL 36:f29a36683b1a 208
sigert 42:b5186a54d839 209 //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 210 //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 211
sigert 42:b5186a54d839 212 //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 213
sigert 42:b5186a54d839 214 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 215 }
ThomasBNL 36:f29a36683b1a 216
ThomasBNL 36:f29a36683b1a 217 void countdown_from_5() // Countdown from 5 till 0 inside Putty (interface)
ThomasBNL 36:f29a36683b1a 218 {
ThomasBNL 36:f29a36683b1a 219 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 220 wait(1); pc.printf("1 Set \n\r"); wait(1); pc.printf("Go \n\r");
ThomasBNL 36:f29a36683b1a 221 }
ThomasBNL 36:f29a36683b1a 222
ThomasBNL 36:f29a36683b1a 223 void calibration()
ThomasBNL 36:f29a36683b1a 224 {
ThomasBNL 36:f29a36683b1a 225
ThomasBNL 36:f29a36683b1a 226 // [MINIMUM VALUE BICEPS CALIBRATION] //
ThomasBNL 36:f29a36683b1a 227
ThomasBNL 36:f29a36683b1a 228 pc.printf("Start minimum calibration in 5 seconds \n\r");
ThomasBNL 36:f29a36683b1a 229 pc.printf("Keep your biceps as relaxed as possible \n\r");
ThomasBNL 36:f29a36683b1a 230
ThomasBNL 36:f29a36683b1a 231 countdown_from_5();
ThomasBNL 36:f29a36683b1a 232 c=0;
ThomasBNL 36:f29a36683b1a 233
ThomasBNL 36:f29a36683b1a 234 while(c<2560) // 512Hz -> 2560 is equal to five seconds
ThomasBNL 36:f29a36683b1a 235 {
ThomasBNL 36:f29a36683b1a 236 Filter(); // Filter EMG signal
ThomasBNL 36:f29a36683b1a 237 minimum_L=EMG_Left_Bicep_filtered+minimum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
ThomasBNL 36:f29a36683b1a 238 minimum_R=EMG_Right_Bicep_filtered+minimum_R;
ThomasBNL 36:f29a36683b1a 239 // scope.set(0,EMG_left_Bicep);
ThomasBNL 36:f29a36683b1a 240 // scope.set(1,EMG_Left_Bicep_filtered);
ThomasBNL 36:f29a36683b1a 241 // scope.set(2,minimum_L);
ThomasBNL 36:f29a36683b1a 242 // scope.send();
ThomasBNL 36:f29a36683b1a 243 c++; // Every sample c is increased by one until the statement c<2560 is false
ThomasBNL 36:f29a36683b1a 244 wait(0.001953125); // wait one sample
ThomasBNL 36:f29a36683b1a 245 }
ThomasBNL 36:f29a36683b1a 246
ThomasBNL 36:f29a36683b1a 247 pc.printf("Finished minimum calibration \n\r");
ThomasBNL 36:f29a36683b1a 248
ThomasBNL 36:f29a36683b1a 249 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 250 EMG_R_min=minimum_R/2560;
ThomasBNL 36:f29a36683b1a 251
ThomasBNL 36:f29a36683b1a 252 pc.printf("EMG_L_min = %f \n\r EMG_R_min = %f \n\r", EMG_L_min, EMG_R_min);
ThomasBNL 36:f29a36683b1a 253
ThomasBNL 36:f29a36683b1a 254 wait (3); //cooldown
ThomasBNL 36:f29a36683b1a 255
ThomasBNL 36:f29a36683b1a 256
ThomasBNL 36:f29a36683b1a 257 // [MAXIMUM VALUE BICEPS CALIBRATION] //
ThomasBNL 36:f29a36683b1a 258
ThomasBNL 36:f29a36683b1a 259
ThomasBNL 36:f29a36683b1a 260 pc.printf("start maximum calibration in 5 seconds (start contraction at 3) \n\r");
ThomasBNL 36:f29a36683b1a 261
ThomasBNL 36:f29a36683b1a 262 countdown_from_5();
ThomasBNL 36:f29a36683b1a 263 c=0;
ThomasBNL 36:f29a36683b1a 264
ThomasBNL 36:f29a36683b1a 265 while(c<2560) // 512Hz -> 2560 is equal to five seconds
ThomasBNL 36:f29a36683b1a 266 {
ThomasBNL 36:f29a36683b1a 267 Filter(); // Filter EMG signal
ThomasBNL 36:f29a36683b1a 268 maximum_L=EMG_Left_Bicep_filtered+maximum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
ThomasBNL 36:f29a36683b1a 269 maximum_R=EMG_Right_Bicep_filtered+maximum_R;
ThomasBNL 36:f29a36683b1a 270 c++; // Every sample c is increased by one until the statement c<2560 is false
ThomasBNL 36:f29a36683b1a 271 wait(0.001953125);
ThomasBNL 36:f29a36683b1a 272 }
ThomasBNL 36:f29a36683b1a 273
ThomasBNL 36:f29a36683b1a 274 pc.printf("Finished minimum calibration \n\r");
ThomasBNL 36:f29a36683b1a 275
ThomasBNL 36:f29a36683b1a 276 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 277 EMG_R_max=maximum_R/2560;
ThomasBNL 36:f29a36683b1a 278
ThomasBNL 36:f29a36683b1a 279 pc.printf("EMG_L_max = %f \n\r EMG_R_max = %f \n\r", EMG_L_max, EMG_R_max);
ThomasBNL 36:f29a36683b1a 280
ThomasBNL 36:f29a36683b1a 281 wait (3); //cooldown
ThomasBNL 36:f29a36683b1a 282
ThomasBNL 36:f29a36683b1a 283
ThomasBNL 36:f29a36683b1a 284 // [MAXIMUM VALUE BICEPS CALIBRATION] //
ThomasBNL 36:f29a36683b1a 285 // Calculate threshold percentages //
ThomasBNL 36:f29a36683b1a 286
sigert 42:b5186a54d839 287 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 288 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 289 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 290 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 291
ThomasBNL 36:f29a36683b1a 292 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 293
ThomasBNL 36:f29a36683b1a 294 }
ThomasBNL 36:f29a36683b1a 295
sigert 37:6c04c15d9bbe 296 void red() { debug_led_red=on; debug_led_blue=off; debug_led_green=off; }
sigert 37:6c04c15d9bbe 297 void blue() { debug_led_red=off; debug_led_blue=on; debug_led_green=off; }
sigert 37:6c04c15d9bbe 298 void green() { debug_led_red=off; debug_led_blue=off; debug_led_green=on; }
sigert 37:6c04c15d9bbe 299 void white() { debug_led_red=on; debug_led_blue=on; debug_led_green=on; }
sigert 37:6c04c15d9bbe 300 void yellow() { debug_led_red=on; debug_led_blue=off; debug_led_green=on; }
sigert 37:6c04c15d9bbe 301 void cyan() { debug_led_red=off; debug_led_blue=on; debug_led_green=on; }
sigert 37:6c04c15d9bbe 302 void purple() { debug_led_red=on; debug_led_blue=on; debug_led_green=off; }
sigert 37:6c04c15d9bbe 303 void black() { debug_led_red=off; debug_led_blue=off; debug_led_green=off; }
ThomasBNL 36:f29a36683b1a 304
sigert 42:b5186a54d839 305 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 306 {
sigert 42:b5186a54d839 307 *in > min ? *in < max? : *in = max: *in = min;
sigert 42:b5186a54d839 308 }