Bouke Scheltinga / Mbed 2 deprecated Werk_college_23sept

Werkcollege opgave 23 september BMT K9

Dependencies:   Encoder HIDScope MODSERIAL mbed QEI biquadFilter

main.cpp@52:45200bbde108, 2015-10-28 (annotated)

Committer:
ThomasBNL
Date:
Wed Oct 28 18:57:19 2015 +0000
Revision:
52:45200bbde108
Parent:
51:b40967b1fe5c 
Programma Step response HIT scope Position (drie seconde full speed ene kant rust drie seconde full speed andere kant) -> kijk uit dat de snoeren niet in de war raken

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"
sigert 46:c88e0d2daf15 6 const double ledon = 1;
sigert 46:c88e0d2daf15 7 const double ledoff = 0;
sigert 46:c88e0d2daf15 8
sigert 46:c88e0d2daf15 9 DigitalOut ledgreen1(D0);
sigert 46:c88e0d2daf15 10 DigitalOut ledgreen2(D1);
sigert 46:c88e0d2daf15 11 DigitalOut ledyellow1(D2);
sigert 46:c88e0d2daf15 12 DigitalOut ledyellow2(D3);
sigert 46:c88e0d2daf15 13 DigitalOut ledred1(D9);
sigert 46:c88e0d2daf15 14 DigitalOut ledred2(D10);
sigert 46:c88e0d2daf15 15
sigert 46:c88e0d2daf15 16 void leduit() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledoff; }
bscheltinga 0:fe3896c6eeb0 17
bscheltinga 26:1090acf98efc 18 // [DEFINE INPUTS] //
sigert 42:b5186a54d839 19 DigitalOut debug_led_red (LED_RED); // Debug LED
sigert 37:6c04c15d9bbe 20 DigitalOut debug_led_green(LED_GREEN); // Debug LED
sigert 42:b5186a54d839 21 DigitalOut debug_led_blue (LED_BLUE); // Debug LED
ThomasBNL 36:f29a36683b1a 22
sigert 37:6c04c15d9bbe 23 const double off=1; // Led off
sigert 37:6c04c15d9bbe 24 const double on=0; // Led on
bscheltinga 21:594915ba2bf9 25
sigert 42:b5186a54d839 26 // MOTORS
sigert 42:b5186a54d839 27 QEI motor_turn(D12,D13,NC,32); QEI motor_strike(D9,D10,NC,32); // TURN - STRIKE : Encoder for motor
sigert 42:b5186a54d839 28 PwmOut pwm_motor_turn(D5); PwmOut pwm_motor_strike(D6); // TURN - STRIKE : Pwm for motor
sigert 42:b5186a54d839 29 DigitalOut motordirection_turn(D4); DigitalOut motordirection_strike(D7); // TURN - STRIKE : Direction of the motor
ThomasBNL 51:b40967b1fe5c 30
sigert 42:b5186a54d839 31
bscheltinga 26:1090acf98efc 32 // [DEFINE CONSTANTS] //
ThomasBNL 52:45200bbde108 33 HIDScope scope(4); // Aantal HIDScope kanalen
bscheltinga 18:68067ffd169e 34 MODSERIAL pc(USBTX,USBRX);
bscheltinga 20:d5f5c60adc43 35 Ticker control_tick;
ThomasBNL 27:85e5d36bb6c5 36 Ticker T1;
bscheltinga 15:7870f7912904 37
bscheltinga 26:1090acf98efc 38 // [BIQUAD FILTERS] //
ThomasBNL 27:85e5d36bb6c5 39 int Fs = 512; // sampling frequency
ThomasBNL 36:f29a36683b1a 40 const double low_b0 = 0.05892937945281792;
ThomasBNL 36:f29a36683b1a 41 const double low_b1 = 0.11785875890563584;
ThomasBNL 36:f29a36683b1a 42 const double low_b2 = 0.05892937945281792;
ThomasBNL 36:f29a36683b1a 43 const double low_a1 = -1.205716572226748;
sigert 37:6c04c15d9bbe 44 const double low_a2 = 0.44143409003801976; //Notch 1 LOW: VIA online biquad calculator Lowpass 520-48-0.7071-6
bscheltinga 15:7870f7912904 45
ThomasBNL 36:f29a36683b1a 46 const double high_b0 = 0.6389437261127494;
ThomasBNL 36:f29a36683b1a 47 const double high_b1 = -1.2778874522254988;
ThomasBNL 36:f29a36683b1a 48 const double high_b2 = 0.6389437261127494;
ThomasBNL 36:f29a36683b1a 49 const double high_a1 = -1.1429772843080923;
ThomasBNL 39:9fa091753592 50 const double high_a2 = 0.41279762014290533; // NOTCH 2 HIGH: VIA online biquad calculator Highpass 520-52-0.7071-6
bscheltinga 22:14abcfdd1554 51
ThomasBNL 39:9fa091753592 52 const double high_b0_HP = 0.84855234544818812;
ThomasBNL 39:9fa091753592 53 const double high_b1_HP = -1.6970469089637623;
ThomasBNL 39:9fa091753592 54 const double high_b2_HP = 0.8485234544818812;
ThomasBNL 39:9fa091753592 55 const double high_a1_HP = -1.6564788473046559;
ThomasBNL 39:9fa091753592 56 const double high_a2_HP = 0.7376149706228688; // HIGHPASS : NOG OPZOEKEN!! : >25Hz? sample rate 512Hz
sigert 37:6c04c15d9bbe 57
ThomasBNL 39:9fa091753592 58 const double low_b0_LP = 0.0013067079602315681;
ThomasBNL 39:9fa091753592 59 const double low_b1_LP = 0.0026134159204631363;
ThomasBNL 39:9fa091753592 60 const double low_b2_LP = 0.0013067079602315681;
ThomasBNL 39:9fa091753592 61 const double low_a1_LP = -1.9238184798980429;
ThomasBNL 39:9fa091753592 62 const double low_a2_LP = 0.9290453117389691; // LOWPASS : NOG OPZOEKEN!! <5-10 Hz? sample rate 512Hz
sigert 37:6c04c15d9bbe 63 //
ThomasBNL 36:f29a36683b1a 64 //Left bicep
sigert 37:6c04c15d9bbe 65 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 66 biquadFilter notchL1(high_a1, high_a2, high_b0, high_b1, high_b2);// moeten nog waardes voor gemaakt worden (>52Hz doorlaten)
ThomasBNL 36:f29a36683b1a 67 biquadFilter notchL2(low_a1, low_a2, low_b0, low_b1, low_b2);// moeten nog waardes voor gemaakt worden (<48Hz doorlaten)
sigert 37:6c04c15d9bbe 68 biquadFilter lowpassfilter_1(low_a1_LP, low_a2_LP, low_b0_LP, low_b1_LP, low_b2_LP);
bscheltinga 0:fe3896c6eeb0 69
ThomasBNL 36:f29a36683b1a 70 // Right bicep
sigert 37:6c04c15d9bbe 71 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 72 biquadFilter notchR1(high_a1, high_a2, high_b0, high_b1, high_b2); // moeten nog waardes voor gemaakt worden
ThomasBNL 36:f29a36683b1a 73 biquadFilter notchR2(low_a1, low_a2, low_b0, low_b1, low_b2); // moeten nog waardes voor gemaakt worden
sigert 37:6c04c15d9bbe 74 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 75
ThomasBNL 27:85e5d36bb6c5 76
ThomasBNL 36:f29a36683b1a 77 AnalogIn input1(A0); // EMG: Two AnalogIn EMG inputs
ThomasBNL 36:f29a36683b1a 78 AnalogIn input2(A1); // EMG: Two AnalogIn EMG inputs
ThomasBNL 36:f29a36683b1a 79
ThomasBNL 36:f29a36683b1a 80 // __________________________
ThomasBNL 36:f29a36683b1a 81 // : [EMG variables] :
ThomasBNL 36:f29a36683b1a 82 // :__________________________:
ThomasBNL 36:f29a36683b1a 83 //
ThomasBNL 36:f29a36683b1a 84
ThomasBNL 36:f29a36683b1a 85 volatile bool control_go = false; // EMG:
ThomasBNL 36:f29a36683b1a 86 volatile bool sample = false;
ThomasBNL 36:f29a36683b1a 87 volatile bool sample_error = false;
ThomasBNL 36:f29a36683b1a 88 volatile bool sample_error_strike = false;
ThomasBNL 36:f29a36683b1a 89
sigert 42:b5186a54d839 90 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 91
ThomasBNL 36:f29a36683b1a 92
ThomasBNL 36:f29a36683b1a 93 double minimum_L; double maximum_L;
ThomasBNL 36:f29a36683b1a 94 double EMG_L_min; double EMG_L_max;
ThomasBNL 36:f29a36683b1a 95 double minimum_R; double maximum_R;
ThomasBNL 36:f29a36683b1a 96 double EMG_R_min; double EMG_R_max;
ThomasBNL 36:f29a36683b1a 97 double EMG_left_Bicep; double EMG_Right_Bicep; // input variables
ThomasBNL 36:f29a36683b1a 98 double EMG_Left_Bicep_filtered_notch_1;double EMG_Right_Bicep_filtered_notch_1;
ThomasBNL 36:f29a36683b1a 99 double EMG_Left_Bicep_filtered_notch_2;double EMG_Right_Bicep_filtered_notch_2;
ThomasBNL 36:f29a36683b1a 100 double EMG_Left_Bicep_filtered; double EMG_Right_Bicep_filtered; // output variables
ThomasBNL 36:f29a36683b1a 101
ThomasBNL 36:f29a36683b1a 102
ThomasBNL 36:f29a36683b1a 103 double n=0; double c=0; double k=0; double p=0; double er=0;
ThomasBNL 36:f29a36683b1a 104
ThomasBNL 36:f29a36683b1a 105 // FUNCTIONS
ThomasBNL 36:f29a36683b1a 106 void ControlGo();
ThomasBNL 36:f29a36683b1a 107 void take_sample();
ThomasBNL 36:f29a36683b1a 108 void Filter();
ThomasBNL 36:f29a36683b1a 109 void sample_filter();
ThomasBNL 36:f29a36683b1a 110 void countdown_from_5();
ThomasBNL 36:f29a36683b1a 111 void calibration();
sigert 37:6c04c15d9bbe 112 void red();void blue();void green();void white();void yellow();void cyan();void purple(); void black();
sigert 37:6c04c15d9bbe 113
sigert 42:b5186a54d839 114 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 115 RS_11,RS_12,RS_13,RS_14,RS_15,RS_16,RS_17,RS_18,RS_19,RS_20,
sigert 42:b5186a54d839 116 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 117
sigert 42:b5186a54d839 118 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 119 LS_11,LS_12,LS_13,LS_14,LS_15,LS_16,LS_17,LS_18,LS_19,LS_20,
sigert 42:b5186a54d839 120 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 121
sigert 42:b5186a54d839 122 double moving_average_right, moving_average_left;
sigert 42:b5186a54d839 123
sigert 42:b5186a54d839 124 void keep_in_range (double * in, double min, double max);
sigert 42:b5186a54d839 125
sigert 45:10ba78d97d75 126 double f; double pwm_average; double smp;
ThomasBNL 52:45200bbde108 127 double position_strike, position_turn, pwm_strike, pwm_turn;
ThomasBNL 52:45200bbde108 128 double conversion_counts_to_degrees=0.085877862594198;
ThomasBNL 36:f29a36683b1a 129 //==========================//
ThomasBNL 27:85e5d36bb6c5 130 // [MAIN FUNCTION] //
ThomasBNL 36:f29a36683b1a 131 //==========================//
ThomasBNL 27:85e5d36bb6c5 132 int main()
ThomasBNL 27:85e5d36bb6c5 133 {
ThomasBNL 27:85e5d36bb6c5 134 control_tick.attach(&ControlGo, (float)1/Fs);
sigert 38:be38b9318849 135 pc.baud(115200);
sigert 42:b5186a54d839 136 //double n=0;
ThomasBNL 52:45200bbde108 137 //calibration(); // calibreer min en max positie
sigert 45:10ba78d97d75 138 f=1;
sigert 45:10ba78d97d75 139 smp=0;
ThomasBNL 27:85e5d36bb6c5 140 while(1)
ThomasBNL 27:85e5d36bb6c5 141 {
ThomasBNL 52:45200bbde108 142 if(f==1)
ThomasBNL 52:45200bbde108 143 {
ThomasBNL 52:45200bbde108 144 pwm_motor_strike=0;
ThomasBNL 52:45200bbde108 145 pwm_motor_turn=0;
ThomasBNL 52:45200bbde108 146 pc.printf("start /n");
ThomasBNL 52:45200bbde108 147 }
ThomasBNL 52:45200bbde108 148 if(f==512)
ThomasBNL 52:45200bbde108 149 {
ThomasBNL 52:45200bbde108 150 pwm_motor_strike=1;
ThomasBNL 52:45200bbde108 151 motordirection_strike=1;
ThomasBNL 52:45200bbde108 152 pwm_motor_turn=1;
ThomasBNL 52:45200bbde108 153 motordirection_turn=1;
ThomasBNL 52:45200bbde108 154 }
ThomasBNL 52:45200bbde108 155
ThomasBNL 52:45200bbde108 156 if(f==2048)
ThomasBNL 52:45200bbde108 157 {
ThomasBNL 52:45200bbde108 158 pc.printf("stop /n");
ThomasBNL 52:45200bbde108 159 pwm_motor_strike=0;
ThomasBNL 52:45200bbde108 160 pwm_motor_turn=0;
ThomasBNL 52:45200bbde108 161 }
ThomasBNL 52:45200bbde108 162
ThomasBNL 52:45200bbde108 163 if(f==2560) // same cycle but referesed direction
ThomasBNL 52:45200bbde108 164 {
ThomasBNL 52:45200bbde108 165 pwm_motor_strike=0;
ThomasBNL 52:45200bbde108 166 pwm_motor_turn=0;
ThomasBNL 52:45200bbde108 167 pc.printf("start /n");
ThomasBNL 52:45200bbde108 168 }
ThomasBNL 52:45200bbde108 169 if(f==3072)
ThomasBNL 52:45200bbde108 170 {
ThomasBNL 52:45200bbde108 171 pwm_motor_strike=1;
ThomasBNL 52:45200bbde108 172 motordirection_strike=0;
ThomasBNL 52:45200bbde108 173 pwm_motor_turn=1;
ThomasBNL 52:45200bbde108 174 motordirection_turn=0;
ThomasBNL 52:45200bbde108 175 }
ThomasBNL 52:45200bbde108 176
ThomasBNL 52:45200bbde108 177 if(f==4500)
ThomasBNL 52:45200bbde108 178 {
ThomasBNL 52:45200bbde108 179 pc.printf("stop /n");
ThomasBNL 52:45200bbde108 180 pwm_motor_strike=0;
ThomasBNL 52:45200bbde108 181 pwm_motor_turn=0;
ThomasBNL 52:45200bbde108 182 f=0;
ThomasBNL 52:45200bbde108 183 }
ThomasBNL 52:45200bbde108 184
sigert 42:b5186a54d839 185 yellow();
ThomasBNL 27:85e5d36bb6c5 186 if(control_go)
ThomasBNL 52:45200bbde108 187 {
sigert 45:10ba78d97d75 188 control_go=0;
ThomasBNL 52:45200bbde108 189 f++;
ThomasBNL 52:45200bbde108 190
ThomasBNL 52:45200bbde108 191 if ((motor_turn.getPulses()>4200) || (motor_turn.getPulses()<-4200)) // If value exceeds -4200 and 4200 (number of counts equal to one revolution) than reset to zero
ThomasBNL 52:45200bbde108 192 { motor_turn.reset(); }
ThomasBNL 52:45200bbde108 193
ThomasBNL 52:45200bbde108 194 position_turn = conversion_counts_to_degrees * motor_turn.getPulses(); // Convert counts to degrees
ThomasBNL 52:45200bbde108 195 pwm_strike=pwm_motor_strike;
ThomasBNL 52:45200bbde108 196 pwm_turn=pwm_motor_turn;
ThomasBNL 52:45200bbde108 197
ThomasBNL 52:45200bbde108 198 if ((motor_strike.getPulses()>4200) || (motor_strike.getPulses()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
ThomasBNL 52:45200bbde108 199 { motor_strike.reset(); }
ThomasBNL 52:45200bbde108 200
ThomasBNL 52:45200bbde108 201 position_strike = conversion_counts_to_degrees * motor_strike.getPulses();
ThomasBNL 52:45200bbde108 202
ThomasBNL 52:45200bbde108 203 scope.set(0,pwm_strike); //ik weet niet of dit weergegeven kan worden
ThomasBNL 52:45200bbde108 204 scope.set(1,pwm_turn); //Filtered signal
ThomasBNL 52:45200bbde108 205 scope.set(2,position_turn); //
ThomasBNL 52:45200bbde108 206 scope.set(3,position_strike); //
ThomasBNL 50:c4c9daf7b74c 207 scope.send();
ThomasBNL 52:45200bbde108 208 }
ThomasBNL 51:b40967b1fe5c 209
sigert 46:c88e0d2daf15 210
ThomasBNL 52:45200bbde108 211
ThomasBNL 52:45200bbde108 212
ThomasBNL 52:45200bbde108 213 // double signal_above_threshold=(moving_average_left); //(moving_average_right-Threshold_Bicep_Right_1)+
ThomasBNL 52:45200bbde108 214 // double max_signal=(EMG_L_max); //(EMG_R_max-Threshold_Bicep_Right_1)+
ThomasBNL 52:45200bbde108 215 // pwm_strike=signal_above_threshold/max_signal;
ThomasBNL 52:45200bbde108 216 // keep_in_range(&pwm_strike, 0,1);
ThomasBNL 52:45200bbde108 217 // pwm_strike=pwm_strike*pwm_strike;
ThomasBNL 52:45200bbde108 218 //
ThomasBNL 52:45200bbde108 219 // pwm_average=pwm_strike+pwm_average/f;
ThomasBNL 52:45200bbde108 220 // f++;
ThomasBNL 52:45200bbde108 221 // smp++;
ThomasBNL 52:45200bbde108 222 // pc.printf("Pwm=%f \n\r", pwm_average);
ThomasBNL 52:45200bbde108 223 // double speed=fabs(pwm_average);
ThomasBNL 52:45200bbde108 224 //
ThomasBNL 52:45200bbde108 225 //
ThomasBNL 52:45200bbde108 226 //if (pwm_average < 0.1) { leduit(); }
ThomasBNL 52:45200bbde108 227 //if (pwm_average > 0.1) { ledgreen1 = ledon; }
ThomasBNL 52:45200bbde108 228 //if (pwm_average > 0.2) { ledgreen2 = ledon; }
ThomasBNL 52:45200bbde108 229 //if (pwm_average > 0.3) { ledyellow1 = ledon; }
ThomasBNL 52:45200bbde108 230 //if (pwm_average > 0.5) { ledyellow2 = ledon; }
ThomasBNL 52:45200bbde108 231 //if (pwm_average > 0.7) { ledred1 = ledon; }
ThomasBNL 52:45200bbde108 232 //if (pwm_average > 0.9) { ledred2 = ledon; }
sigert 46:c88e0d2daf15 233
ThomasBNL 52:45200bbde108 234 // if(smp>512)
ThomasBNL 52:45200bbde108 235 // {
ThomasBNL 52:45200bbde108 236 // pwm_motor_strike=speed;
ThomasBNL 52:45200bbde108 237 // pwm_motor_turn=speed;
ThomasBNL 52:45200bbde108 238 // green();
ThomasBNL 52:45200bbde108 239 // pwm_motor_strike=0;
ThomasBNL 52:45200bbde108 240 // wait(3);
ThomasBNL 52:45200bbde108 241 // f=1;
ThomasBNL 52:45200bbde108 242 // smp=0;}
ThomasBNL 52:45200bbde108 243 // }
ThomasBNL 27:85e5d36bb6c5 244 } // while end
ThomasBNL 27:85e5d36bb6c5 245 } // main end
ThomasBNL 36:f29a36683b1a 246
ThomasBNL 36:f29a36683b1a 247 // --------------------------------------------------------------------------------------------------------
ThomasBNL 36:f29a36683b1a 248 // [FUNCTIONS] //
ThomasBNL 36:f29a36683b1a 249 void ControlGo() //Control flag
ThomasBNL 36:f29a36683b1a 250 {
ThomasBNL 36:f29a36683b1a 251 control_go = true;
ThomasBNL 36:f29a36683b1a 252 }
ThomasBNL 36:f29a36683b1a 253
ThomasBNL 36:f29a36683b1a 254 void sample_filter()
ThomasBNL 36:f29a36683b1a 255 {
ThomasBNL 36:f29a36683b1a 256 Filter();
ThomasBNL 36:f29a36683b1a 257 take_sample();
ThomasBNL 36:f29a36683b1a 258 if(sample)
ThomasBNL 36:f29a36683b1a 259 {
ThomasBNL 36:f29a36683b1a 260 sample=false;
sigert 42:b5186a54d839 261 LS_0 = EMG_Left_Bicep_filtered; RS_0 = EMG_Right_Bicep_filtered;
sigert 42:b5186a54d839 262
sigert 42:b5186a54d839 263 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 264 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 265 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 266
sigert 42:b5186a54d839 267 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 268 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 269 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 270
ThomasBNL 36:f29a36683b1a 271 }
sigert 42:b5186a54d839 272 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 273 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 274 n++;
ThomasBNL 36:f29a36683b1a 275 }
ThomasBNL 36:f29a36683b1a 276
ThomasBNL 36:f29a36683b1a 277 void take_sample() // Take a sample every 25th sample
ThomasBNL 36:f29a36683b1a 278 {
sigert 42:b5186a54d839 279 if(n==8)
ThomasBNL 36:f29a36683b1a 280 {
ThomasBNL 36:f29a36683b1a 281 sample = true; n=0;
ThomasBNL 36:f29a36683b1a 282 }
ThomasBNL 36:f29a36683b1a 283
ThomasBNL 36:f29a36683b1a 284 if(er==5)
ThomasBNL 36:f29a36683b1a 285 {
ThomasBNL 36:f29a36683b1a 286 sample_error = true; er=0;
ThomasBNL 36:f29a36683b1a 287 }
ThomasBNL 36:f29a36683b1a 288
ThomasBNL 36:f29a36683b1a 289 sample_error_strike = true;
ThomasBNL 36:f29a36683b1a 290 }
ThomasBNL 36:f29a36683b1a 291
ThomasBNL 36:f29a36683b1a 292 // [FILTER FUNCTIONS] //
ThomasBNL 36:f29a36683b1a 293 // [EMG] //
ThomasBNL 36:f29a36683b1a 294
ThomasBNL 36:f29a36683b1a 295 void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output)
ThomasBNL 36:f29a36683b1a 296 {
ThomasBNL 51:b40967b1fe5c 297
ThomasBNL 51:b40967b1fe5c 298 //ZONDER NOTCH
ThomasBNL 36:f29a36683b1a 299 EMG_left_Bicep = input1; EMG_Right_Bicep = input2;
ThomasBNL 36:f29a36683b1a 300
ThomasBNL 36:f29a36683b1a 301 EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep);
ThomasBNL 36:f29a36683b1a 302 EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered);
ThomasBNL 51:b40967b1fe5c 303
ThomasBNL 51:b40967b1fe5c 304 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 305
ThomasBNL 36:f29a36683b1a 306
ThomasBNL 51:b40967b1fe5c 307 // EMG_left_Bicep = input1; EMG_Right_Bicep = input2;
ThomasBNL 51:b40967b1fe5c 308 //
ThomasBNL 51:b40967b1fe5c 309 // EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep);
ThomasBNL 51:b40967b1fe5c 310 //
ThomasBNL 51:b40967b1fe5c 311 // EMG_Left_Bicep_filtered_notch_1 = notchL1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered_notch_1 = notchR1.step(EMG_Right_Bicep_filtered);
ThomasBNL 51:b40967b1fe5c 312 // 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 51:b40967b1fe5c 313 //
ThomasBNL 51:b40967b1fe5c 314 // 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);
ThomasBNL 51:b40967b1fe5c 315 //
ThomasBNL 51:b40967b1fe5c 316 //
ThomasBNL 51:b40967b1fe5c 317 // EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered);
ThomasBNL 51:b40967b1fe5c 318 //
ThomasBNL 51:b40967b1fe5c 319 // 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 320 }
ThomasBNL 36:f29a36683b1a 321
ThomasBNL 36:f29a36683b1a 322 void countdown_from_5() // Countdown from 5 till 0 inside Putty (interface)
ThomasBNL 36:f29a36683b1a 323 {
ThomasBNL 36:f29a36683b1a 324 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 325 wait(1); pc.printf("1 Set \n\r"); wait(1); pc.printf("Go \n\r");
ThomasBNL 36:f29a36683b1a 326 }
ThomasBNL 36:f29a36683b1a 327
ThomasBNL 36:f29a36683b1a 328 void calibration()
ThomasBNL 36:f29a36683b1a 329 {
ThomasBNL 36:f29a36683b1a 330
ThomasBNL 36:f29a36683b1a 331 // [MINIMUM VALUE BICEPS CALIBRATION] //
ThomasBNL 36:f29a36683b1a 332
ThomasBNL 36:f29a36683b1a 333 pc.printf("Start minimum calibration in 5 seconds \n\r");
ThomasBNL 36:f29a36683b1a 334 pc.printf("Keep your biceps as relaxed as possible \n\r");
ThomasBNL 36:f29a36683b1a 335
ThomasBNL 36:f29a36683b1a 336 countdown_from_5();
ThomasBNL 36:f29a36683b1a 337 c=0;
ThomasBNL 36:f29a36683b1a 338
ThomasBNL 36:f29a36683b1a 339 while(c<2560) // 512Hz -> 2560 is equal to five seconds
ThomasBNL 36:f29a36683b1a 340 {
ThomasBNL 36:f29a36683b1a 341 Filter(); // Filter EMG signal
ThomasBNL 36:f29a36683b1a 342 minimum_L=EMG_Left_Bicep_filtered+minimum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
ThomasBNL 36:f29a36683b1a 343 minimum_R=EMG_Right_Bicep_filtered+minimum_R;
ThomasBNL 36:f29a36683b1a 344 // scope.set(0,EMG_left_Bicep);
ThomasBNL 36:f29a36683b1a 345 // scope.set(1,EMG_Left_Bicep_filtered);
ThomasBNL 36:f29a36683b1a 346 // scope.set(2,minimum_L);
ThomasBNL 36:f29a36683b1a 347 // scope.send();
ThomasBNL 36:f29a36683b1a 348 c++; // Every sample c is increased by one until the statement c<2560 is false
ThomasBNL 36:f29a36683b1a 349 wait(0.001953125); // wait one sample
ThomasBNL 36:f29a36683b1a 350 }
ThomasBNL 36:f29a36683b1a 351
ThomasBNL 36:f29a36683b1a 352 pc.printf("Finished minimum calibration \n\r");
ThomasBNL 36:f29a36683b1a 353
ThomasBNL 36:f29a36683b1a 354 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 355 EMG_R_min=minimum_R/2560;
ThomasBNL 36:f29a36683b1a 356
ThomasBNL 36:f29a36683b1a 357 pc.printf("EMG_L_min = %f \n\r EMG_R_min = %f \n\r", EMG_L_min, EMG_R_min);
ThomasBNL 36:f29a36683b1a 358
ThomasBNL 36:f29a36683b1a 359 wait (3); //cooldown
ThomasBNL 36:f29a36683b1a 360
ThomasBNL 36:f29a36683b1a 361
ThomasBNL 36:f29a36683b1a 362 // [MAXIMUM VALUE BICEPS CALIBRATION] //
ThomasBNL 36:f29a36683b1a 363
ThomasBNL 36:f29a36683b1a 364
ThomasBNL 36:f29a36683b1a 365 pc.printf("start maximum calibration in 5 seconds (start contraction at 3) \n\r");
ThomasBNL 36:f29a36683b1a 366
ThomasBNL 36:f29a36683b1a 367 countdown_from_5();
ThomasBNL 36:f29a36683b1a 368 c=0;
ThomasBNL 36:f29a36683b1a 369
ThomasBNL 36:f29a36683b1a 370 while(c<2560) // 512Hz -> 2560 is equal to five seconds
ThomasBNL 36:f29a36683b1a 371 {
ThomasBNL 36:f29a36683b1a 372 Filter(); // Filter EMG signal
ThomasBNL 36:f29a36683b1a 373 maximum_L=EMG_Left_Bicep_filtered+maximum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
ThomasBNL 36:f29a36683b1a 374 maximum_R=EMG_Right_Bicep_filtered+maximum_R;
ThomasBNL 36:f29a36683b1a 375 c++; // Every sample c is increased by one until the statement c<2560 is false
ThomasBNL 36:f29a36683b1a 376 wait(0.001953125);
ThomasBNL 36:f29a36683b1a 377 }
ThomasBNL 36:f29a36683b1a 378
ThomasBNL 36:f29a36683b1a 379 pc.printf("Finished minimum calibration \n\r");
ThomasBNL 36:f29a36683b1a 380
ThomasBNL 36:f29a36683b1a 381 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 382 EMG_R_max=maximum_R/2560;
ThomasBNL 36:f29a36683b1a 383
ThomasBNL 36:f29a36683b1a 384 pc.printf("EMG_L_max = %f \n\r EMG_R_max = %f \n\r", EMG_L_max, EMG_R_max);
ThomasBNL 36:f29a36683b1a 385
ThomasBNL 36:f29a36683b1a 386 wait (3); //cooldown
ThomasBNL 36:f29a36683b1a 387
ThomasBNL 36:f29a36683b1a 388
ThomasBNL 36:f29a36683b1a 389 // [MAXIMUM VALUE BICEPS CALIBRATION] //
ThomasBNL 36:f29a36683b1a 390 // Calculate threshold percentages //
ThomasBNL 36:f29a36683b1a 391
sigert 42:b5186a54d839 392 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 393 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 394 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 395 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 396
ThomasBNL 36:f29a36683b1a 397 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 398
ThomasBNL 36:f29a36683b1a 399 }
ThomasBNL 36:f29a36683b1a 400
sigert 37:6c04c15d9bbe 401 void red() { debug_led_red=on; debug_led_blue=off; debug_led_green=off; }
sigert 37:6c04c15d9bbe 402 void blue() { debug_led_red=off; debug_led_blue=on; debug_led_green=off; }
sigert 37:6c04c15d9bbe 403 void green() { debug_led_red=off; debug_led_blue=off; debug_led_green=on; }
sigert 37:6c04c15d9bbe 404 void white() { debug_led_red=on; debug_led_blue=on; debug_led_green=on; }
sigert 37:6c04c15d9bbe 405 void yellow() { debug_led_red=on; debug_led_blue=off; debug_led_green=on; }
sigert 37:6c04c15d9bbe 406 void cyan() { debug_led_red=off; debug_led_blue=on; debug_led_green=on; }
sigert 37:6c04c15d9bbe 407 void purple() { debug_led_red=on; debug_led_blue=on; debug_led_green=off; }
sigert 37:6c04c15d9bbe 408 void black() { debug_led_red=off; debug_led_blue=off; debug_led_green=off; }
ThomasBNL 36:f29a36683b1a 409
sigert 42:b5186a54d839 410 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 411 {
sigert 42:b5186a54d839 412 *in > min ? *in < max? : *in = max: *in = min;
sigert 42:b5186a54d839 413 }