![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
final version
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of Project_script_union_final by
main.cpp@23:97a976a8f0fc, 2018-10-30 (annotated)
- Committer:
- MarijkeZondag
- Date:
- Tue Oct 30 10:37:45 2018 +0000
- Revision:
- 23:97a976a8f0fc
- Parent:
- 22:5d956c93b3ae
- Child:
- 24:6d63ad38fef7
Versie 30-10-2018
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:c8f15874531b | 1 | #include "mbed.h" |
vsluiter | 0:c8f15874531b | 2 | #include "MODSERIAL.h" |
MarijkeZondag | 10:39ec51206c8b | 3 | #include "BiQuad.h" |
MarijkeZondag | 10:39ec51206c8b | 4 | #include "HIDScope.h" |
MarijkeZondag | 10:39ec51206c8b | 5 | #include <math.h> |
vsluiter | 0:c8f15874531b | 6 | |
MarijkeZondag | 22:5d956c93b3ae | 7 | //ATTENTION: set mBed to version 151 |
MarijkeZondag | 22:5d956c93b3ae | 8 | // set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder) |
MarijkeZondag | 22:5d956c93b3ae | 9 | // set MODSERIAL to version 44 |
MarijkeZondag | 22:5d956c93b3ae | 10 | // set HIDScope to version 7 |
MarijkeZondag | 22:5d956c93b3ae | 11 | // set biquadFilter to version 7 |
MarijkeZondag | 10:39ec51206c8b | 12 | |
MarijkeZondag | 22:5d956c93b3ae | 13 | AnalogIn emg0_in (A0); //First raw EMG signal input |
MarijkeZondag | 22:5d956c93b3ae | 14 | AnalogIn emg1_in (A1); //Second raw EMG signal input |
MarijkeZondag | 22:5d956c93b3ae | 15 | AnalogIn emg2_in (A2); //Third raw EMG signal input |
MarijkeZondag | 22:5d956c93b3ae | 16 | |
MarijkeZondag | 22:5d956c93b3ae | 17 | InterruptIn button1 (D10); //Is this one available? We need to make a map of which pins are used for what. |
MarijkeZondag | 16:5f7196ddc77b | 18 | InterruptIn button2 (D11); |
MarijkeZondag | 6:f4bbb73f3989 | 19 | |
MarijkeZondag | 23:97a976a8f0fc | 20 | DigitalOut directionpin1 (D7); |
MarijkeZondag | 23:97a976a8f0fc | 21 | DigitalOut directionpin2 (D4); |
MarijkeZondag | 23:97a976a8f0fc | 22 | PwmOut pwmpin1 (D6); |
MarijkeZondag | 23:97a976a8f0fc | 23 | PwmOut pwmpin2 (D5); |
MarijkeZondag | 22:5d956c93b3ae | 24 | |
MarijkeZondag | 17:741798018c0d | 25 | DigitalOut ledr (LED_RED); |
MarijkeZondag | 17:741798018c0d | 26 | DigitalOut ledb (LED_BLUE); |
MarijkeZondag | 17:741798018c0d | 27 | DigitalOut ledg (LED_GREEN); |
MarijkeZondag | 13:a3d4b4daf5b4 | 28 | |
MarijkeZondag | 9:c722418997b5 | 29 | |
MarijkeZondag | 23:97a976a8f0fc | 30 | MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off |
MarijkeZondag | 6:f4bbb73f3989 | 31 | |
MarijkeZondag | 23:97a976a8f0fc | 32 | //HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered |
vsluiter | 0:c8f15874531b | 33 | |
MarijkeZondag | 22:5d956c93b3ae | 34 | //Tickers |
MarijkeZondag | 22:5d956c93b3ae | 35 | Ticker HIDScope_tick; //Ticker for HIDScope |
MarijkeZondag | 22:5d956c93b3ae | 36 | Ticker filter_tick; //Ticker for EMG filter |
MarijkeZondag | 22:5d956c93b3ae | 37 | Ticker MovAg_tick; //Ticker to calculate Moving Average |
MarijkeZondag | 23:97a976a8f0fc | 38 | Ticker Motor_tick; //Ticker motor aansturen |
MarijkeZondag | 9:c722418997b5 | 39 | |
MarijkeZondag | 9:c722418997b5 | 40 | //Global variables |
MarijkeZondag | 22:5d956c93b3ae | 41 | const float T = 0.002f; //Ticker period |
MarijkeZondag | 10:39ec51206c8b | 42 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 43 | //EMG filter |
MarijkeZondag | 22:5d956c93b3ae | 44 | double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2 |
MarijkeZondag | 22:5d956c93b3ae | 45 | double emg0_raw, emg1_raw, emg2_raw; |
MarijkeZondag | 22:5d956c93b3ae | 46 | double emg0_filt_x, emg1_filt_x, emg2_filt_x; |
MarijkeZondag | 22:5d956c93b3ae | 47 | const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated. (random number) |
MarijkeZondag | 13:a3d4b4daf5b4 | 48 | double sum, sum1, sum2, sum3; //variables used to sum elements in array |
MarijkeZondag | 22:5d956c93b3ae | 49 | double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg |
MarijkeZondag | 22:5d956c93b3ae | 50 | double movAg0, movAg1, movAg2; //outcome of MovAg (moet dit een array zijn??) |
MarijkeZondag | 13:a3d4b4daf5b4 | 51 | |
MarijkeZondag | 22:5d956c93b3ae | 52 | //Calibration variables |
MarijkeZondag | 22:5d956c93b3ae | 53 | int x = -1; //Start switch, colour LED is blue. |
MarijkeZondag | 22:5d956c93b3ae | 54 | int emg_cal = 0; //if emg_cal is set to 1, motors can begin to work in this code (!!) |
MarijkeZondag | 22:5d956c93b3ae | 55 | const int sizeCal = 1500; //size of the dataset used for calibration, eerst 2000 |
MarijkeZondag | 22:5d956c93b3ae | 56 | double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //arrays to put the dataset of the calibration in |
MarijkeZondag | 22:5d956c93b3ae | 57 | double Mean0,Mean1,Mean2; //average of maximum tightening |
MarijkeZondag | 22:5d956c93b3ae | 58 | double Threshold0, Threshold1, Threshold2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 59 | |
MarijkeZondag | 22:5d956c93b3ae | 60 | //Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo) |
MarijkeZondag | 22:5d956c93b3ae | 61 | BiQuadChain emg0filter; |
MarijkeZondag | 10:39ec51206c8b | 62 | BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 63 | BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 64 | BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 22:5d956c93b3ae | 65 | BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients |
MarijkeZondag | 10:39ec51206c8b | 66 | |
MarijkeZondag | 22:5d956c93b3ae | 67 | BiQuadChain emg1filter; |
MarijkeZondag | 10:39ec51206c8b | 68 | BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 69 | BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 70 | BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 22:5d956c93b3ae | 71 | BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter |
MarijkeZondag | 10:39ec51206c8b | 72 | |
MarijkeZondag | 22:5d956c93b3ae | 73 | BiQuadChain emg2filter; |
MarijkeZondag | 10:39ec51206c8b | 74 | BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 75 | BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 76 | BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 14:fa09dae67390 | 77 | BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter |
MarijkeZondag | 10:39ec51206c8b | 78 | |
MarijkeZondag | 9:c722418997b5 | 79 | //Functions |
MarijkeZondag | 12:eaed305a76c3 | 80 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 81 | void switch_to_calibrate() |
MarijkeZondag | 13:a3d4b4daf5b4 | 82 | { |
MarijkeZondag | 22:5d956c93b3ae | 83 | x++; //Every time function gets called, x increases. Every button press --> new calibration state. |
MarijkeZondag | 22:5d956c93b3ae | 84 | //Starts with x = -1. So when function gets called 1 time, x = 0. In the end, x = 4 will reset to -1. |
MarijkeZondag | 22:5d956c93b3ae | 85 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 86 | if(x==0) //If x = 0, led is red |
MarijkeZondag | 13:a3d4b4daf5b4 | 87 | { |
MarijkeZondag | 17:741798018c0d | 88 | ledr = 0; |
MarijkeZondag | 17:741798018c0d | 89 | ledb = 1; |
MarijkeZondag | 17:741798018c0d | 90 | ledg = 1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 91 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 92 | else if (x==1) //If x = 1, led is blue |
MarijkeZondag | 13:a3d4b4daf5b4 | 93 | { |
MarijkeZondag | 17:741798018c0d | 94 | ledr = 1; |
MarijkeZondag | 17:741798018c0d | 95 | ledb = 0; |
MarijkeZondag | 17:741798018c0d | 96 | ledg = 1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 97 | } |
MarijkeZondag | 22:5d956c93b3ae | 98 | else if (x==2) //If x = 2, led is green |
MarijkeZondag | 13:a3d4b4daf5b4 | 99 | { |
MarijkeZondag | 17:741798018c0d | 100 | ledr = 1; |
MarijkeZondag | 17:741798018c0d | 101 | ledb = 1; |
MarijkeZondag | 17:741798018c0d | 102 | ledg = 0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 103 | } |
MarijkeZondag | 22:5d956c93b3ae | 104 | else //If x = 3 or 4, led is white |
MarijkeZondag | 13:a3d4b4daf5b4 | 105 | { |
MarijkeZondag | 17:741798018c0d | 106 | ledr = 0; |
MarijkeZondag | 17:741798018c0d | 107 | ledb = 0; |
MarijkeZondag | 17:741798018c0d | 108 | ledg = 0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 109 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 110 | |
MarijkeZondag | 22:5d956c93b3ae | 111 | if(x==4) //Reset back to x = -1 |
MarijkeZondag | 13:a3d4b4daf5b4 | 112 | { |
MarijkeZondag | 16:5f7196ddc77b | 113 | x = -1; |
MarijkeZondag | 23:97a976a8f0fc | 114 | emg_cal=0; //reset, motors off |
MarijkeZondag | 13:a3d4b4daf5b4 | 115 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 116 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 117 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 118 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 119 | void calibrate(void) |
MarijkeZondag | 12:eaed305a76c3 | 120 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 121 | switch(x) |
MarijkeZondag | 13:a3d4b4daf5b4 | 122 | { |
MarijkeZondag | 22:5d956c93b3ae | 123 | case 0: //If calibration state 0: |
MarijkeZondag | 13:a3d4b4daf5b4 | 124 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 125 | sum = 0.0; |
MarijkeZondag | 22:5d956c93b3ae | 126 | for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0 |
MarijkeZondag | 13:a3d4b4daf5b4 | 127 | { |
MarijkeZondag | 22:5d956c93b3ae | 128 | StoreCal0[j] = emg0_filt; |
MarijkeZondag | 13:a3d4b4daf5b4 | 129 | sum+=StoreCal0[j]; |
MarijkeZondag | 22:5d956c93b3ae | 130 | wait(0.001f); //Does there need to be a wait? |
MarijkeZondag | 13:a3d4b4daf5b4 | 131 | } |
MarijkeZondag | 22:5d956c93b3ae | 132 | Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples) |
MarijkeZondag | 23:97a976a8f0fc | 133 | Threshold0 = Mean0/2; //Threshold calculation = 0.5*mean |
MarijkeZondag | 22:5d956c93b3ae | 134 | break; //Stop. Threshold is calculated, we will use this further in the code |
MarijkeZondag | 13:a3d4b4daf5b4 | 135 | } |
MarijkeZondag | 22:5d956c93b3ae | 136 | case 1: //If calibration state 1: |
MarijkeZondag | 13:a3d4b4daf5b4 | 137 | { |
MarijkeZondag | 22:5d956c93b3ae | 138 | sum = 0.0; |
MarijkeZondag | 22:5d956c93b3ae | 139 | for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1 |
MarijkeZondag | 13:a3d4b4daf5b4 | 140 | { |
MarijkeZondag | 22:5d956c93b3ae | 141 | StoreCal1[j] = emg1_filt; |
MarijkeZondag | 13:a3d4b4daf5b4 | 142 | sum+=StoreCal1[j]; |
MarijkeZondag | 21:1da43fdbd254 | 143 | wait(0.001f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 144 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 145 | Mean1 = sum/sizeCal; |
MarijkeZondag | 23:97a976a8f0fc | 146 | Threshold1 = Mean1/2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 147 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 148 | } |
MarijkeZondag | 22:5d956c93b3ae | 149 | case 2: //If calibration state 2: |
MarijkeZondag | 13:a3d4b4daf5b4 | 150 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 151 | sum = 0.0; |
MarijkeZondag | 22:5d956c93b3ae | 152 | for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2 |
MarijkeZondag | 13:a3d4b4daf5b4 | 153 | { |
MarijkeZondag | 23:97a976a8f0fc | 154 | StoreCal2[j] = emg2_filt; |
MarijkeZondag | 13:a3d4b4daf5b4 | 155 | sum+=StoreCal2[j]; |
MarijkeZondag | 21:1da43fdbd254 | 156 | wait(0.001f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 157 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 158 | Mean2 = sum/sizeCal; |
MarijkeZondag | 23:97a976a8f0fc | 159 | Threshold2 = Mean2/2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 160 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 161 | } |
MarijkeZondag | 22:5d956c93b3ae | 162 | case 3: //EMG is calibrated, robot can be set to Home position. |
MarijkeZondag | 13:a3d4b4daf5b4 | 163 | { |
MarijkeZondag | 22:5d956c93b3ae | 164 | emg_cal = 1; //This is the setting for which the motors can begin turning in this code (!!) |
MarijkeZondag | 21:1da43fdbd254 | 165 | wait(0.001f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 166 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 167 | } |
MarijkeZondag | 22:5d956c93b3ae | 168 | default: //Ensures nothing happens if x is not 0,1 or 2. |
MarijkeZondag | 13:a3d4b4daf5b4 | 169 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 170 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 171 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 172 | } |
MarijkeZondag | 12:eaed305a76c3 | 173 | } |
MarijkeZondag | 22:5d956c93b3ae | 174 | |
MarijkeZondag | 22:5d956c93b3ae | 175 | void HIDScope_sample() |
MarijkeZondag | 22:5d956c93b3ae | 176 | { |
MarijkeZondag | 22:5d956c93b3ae | 177 | /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ |
MarijkeZondag | 23:97a976a8f0fc | 178 | //scope.set(0,emg0_raw); |
MarijkeZondag | 22:5d956c93b3ae | 179 | //scope.set(1,emg0_filt); |
MarijkeZondag | 23:97a976a8f0fc | 180 | //scope.set(1,movAg0); //als moving average werkt |
MarijkeZondag | 23:97a976a8f0fc | 181 | //scope.set(2,emg1_raw); |
MarijkeZondag | 22:5d956c93b3ae | 182 | //scope.set(3,emg1_filt); |
MarijkeZondag | 23:97a976a8f0fc | 183 | //scope.set(3,movAg1); //als moving average werkt |
MarijkeZondag | 23:97a976a8f0fc | 184 | //scope.set(4,emg2_raw); |
MarijkeZondag | 22:5d956c93b3ae | 185 | //scope.set(5,emg2_filt); |
MarijkeZondag | 23:97a976a8f0fc | 186 | //scope.set(5,movAg2); //als moving average werkt |
MarijkeZondag | 22:5d956c93b3ae | 187 | |
MarijkeZondag | 23:97a976a8f0fc | 188 | //scope.send(); //Send data to HIDScope server |
MarijkeZondag | 22:5d956c93b3ae | 189 | } |
MarijkeZondag | 22:5d956c93b3ae | 190 | |
MarijkeZondag | 22:5d956c93b3ae | 191 | void EMGFilter0() |
MarijkeZondag | 22:5d956c93b3ae | 192 | { |
MarijkeZondag | 22:5d956c93b3ae | 193 | emg0_raw = emg0_in.read(); //give name to raw EMG0 data |
MarijkeZondag | 22:5d956c93b3ae | 194 | emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data |
MarijkeZondag | 22:5d956c93b3ae | 195 | emg0_filt = abs(emg0_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch, stel er komt iets raars uit, dan Notch uit de biquad chain halen en aparte chain voor aanmaken. |
MarijkeZondag | 22:5d956c93b3ae | 196 | } |
MarijkeZondag | 22:5d956c93b3ae | 197 | |
MarijkeZondag | 22:5d956c93b3ae | 198 | void EMGFilter1() |
MarijkeZondag | 8:895d941a5910 | 199 | { |
MarijkeZondag | 22:5d956c93b3ae | 200 | emg1_raw = emg1_in.read(); //give name to raw EMG1 data |
MarijkeZondag | 22:5d956c93b3ae | 201 | emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data |
MarijkeZondag | 22:5d956c93b3ae | 202 | emg1_filt = abs(emg1_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch. |
MarijkeZondag | 8:895d941a5910 | 203 | } |
MarijkeZondag | 8:895d941a5910 | 204 | |
MarijkeZondag | 22:5d956c93b3ae | 205 | void EMGFilter2() |
MarijkeZondag | 8:895d941a5910 | 206 | { |
MarijkeZondag | 22:5d956c93b3ae | 207 | emg2_raw = emg2_in.read(); //Give name to raw EMG1 data |
MarijkeZondag | 22:5d956c93b3ae | 208 | emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data |
MarijkeZondag | 22:5d956c93b3ae | 209 | emg2_filt = abs(emg2_filt_x); //Rectifier. LET OP: volgorde filter: band-notch-rectifier. |
MarijkeZondag | 22:5d956c93b3ae | 210 | } |
MarijkeZondag | 22:5d956c93b3ae | 211 | |
MarijkeZondag | 22:5d956c93b3ae | 212 | void MovAg() //Calculate moving average (MovAg), klopt nog niet!! |
MarijkeZondag | 22:5d956c93b3ae | 213 | { |
MarijkeZondag | 22:5d956c93b3ae | 214 | for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals |
MarijkeZondag | 8:895d941a5910 | 215 | { |
MarijkeZondag | 22:5d956c93b3ae | 216 | StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average. |
MarijkeZondag | 22:5d956c93b3ae | 217 | StoreArray1[i] = StoreArray1[i-1]; |
MarijkeZondag | 22:5d956c93b3ae | 218 | StoreArray2[i] = StoreArray2[i-1]; |
MarijkeZondag | 8:895d941a5910 | 219 | } |
MarijkeZondag | 22:5d956c93b3ae | 220 | |
MarijkeZondag | 22:5d956c93b3ae | 221 | StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array |
MarijkeZondag | 22:5d956c93b3ae | 222 | StoreArray1[0] = emg1_filt; |
MarijkeZondag | 22:5d956c93b3ae | 223 | StoreArray2[0] = emg2_filt; |
MarijkeZondag | 22:5d956c93b3ae | 224 | |
MarijkeZondag | 22:5d956c93b3ae | 225 | sum1 = 0.0; |
MarijkeZondag | 22:5d956c93b3ae | 226 | sum2 = 0.0; |
MarijkeZondag | 22:5d956c93b3ae | 227 | sum3 = 0.0; |
MarijkeZondag | 22:5d956c93b3ae | 228 | |
MarijkeZondag | 22:5d956c93b3ae | 229 | for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays |
MarijkeZondag | 8:895d941a5910 | 230 | { |
MarijkeZondag | 22:5d956c93b3ae | 231 | sum1 += StoreArray0[a]; |
MarijkeZondag | 22:5d956c93b3ae | 232 | sum2 += StoreArray1[a]; |
MarijkeZondag | 22:5d956c93b3ae | 233 | sum3 += StoreArray2[a]; |
MarijkeZondag | 8:895d941a5910 | 234 | } |
MarijkeZondag | 22:5d956c93b3ae | 235 | |
MarijkeZondag | 22:5d956c93b3ae | 236 | movAg0 = sum1/windowsize; //calculates an average in the array |
MarijkeZondag | 22:5d956c93b3ae | 237 | movAg1 = sum2/windowsize; |
MarijkeZondag | 22:5d956c93b3ae | 238 | movAg2 = sum3/windowsize; |
MarijkeZondag | 22:5d956c93b3ae | 239 | //serial getallen sturen, als het 1 getal is gaat hier wat fout, als het een reeks is dan gaat er bij de input naar HIDscope wat fout. |
MarijkeZondag | 8:895d941a5910 | 240 | } |
MarijkeZondag | 8:895d941a5910 | 241 | |
MarijkeZondag | 22:5d956c93b3ae | 242 | void emg_filtered() //Call all filter functions |
MarijkeZondag | 8:895d941a5910 | 243 | { |
MarijkeZondag | 22:5d956c93b3ae | 244 | EMGFilter0(); |
MarijkeZondag | 22:5d956c93b3ae | 245 | EMGFilter1(); |
MarijkeZondag | 22:5d956c93b3ae | 246 | EMGFilter2(); |
MarijkeZondag | 8:895d941a5910 | 247 | } |
MarijkeZondag | 8:895d941a5910 | 248 | |
MarijkeZondag | 23:97a976a8f0fc | 249 | void motor_control() |
MarijkeZondag | 23:97a976a8f0fc | 250 | { |
MarijkeZondag | 23:97a976a8f0fc | 251 | while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0. |
MarijkeZondag | 16:5f7196ddc77b | 252 | { |
MarijkeZondag | 23:97a976a8f0fc | 253 | // pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2); |
MarijkeZondag | 23:97a976a8f0fc | 254 | // pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2); |
MarijkeZondag | 23:97a976a8f0fc | 255 | // wait(2.0f); |
MarijkeZondag | 22:5d956c93b3ae | 256 | |
MarijkeZondag | 22:5d956c93b3ae | 257 | if(movAg0>Threshold0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction |
MarijkeZondag | 16:5f7196ddc77b | 258 | { |
MarijkeZondag | 16:5f7196ddc77b | 259 | pwmpin1 = 1; |
MarijkeZondag | 23:97a976a8f0fc | 260 | directionpin1.write(1); //translatie vooruit |
MarijkeZondag | 22:5d956c93b3ae | 261 | |
MarijkeZondag | 22:5d956c93b3ae | 262 | ledr = 1; //Blue |
MarijkeZondag | 22:5d956c93b3ae | 263 | ledb = 0; |
MarijkeZondag | 22:5d956c93b3ae | 264 | ledg = 1; |
MarijkeZondag | 22:5d956c93b3ae | 265 | |
MarijkeZondag | 16:5f7196ddc77b | 266 | } |
MarijkeZondag | 22:5d956c93b3ae | 267 | else //If it is not higher than the threshold, the motor will not turn at all. |
MarijkeZondag | 16:5f7196ddc77b | 268 | { |
MarijkeZondag | 16:5f7196ddc77b | 269 | pwmpin1 = 0; |
MarijkeZondag | 22:5d956c93b3ae | 270 | ledr = 0; //white |
MarijkeZondag | 22:5d956c93b3ae | 271 | ledb = 0; |
MarijkeZondag | 22:5d956c93b3ae | 272 | ledg = 0; |
MarijkeZondag | 16:5f7196ddc77b | 273 | } |
MarijkeZondag | 9:c722418997b5 | 274 | |
MarijkeZondag | 22:5d956c93b3ae | 275 | if(movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor2 will turn in 1 direction |
MarijkeZondag | 16:5f7196ddc77b | 276 | { |
MarijkeZondag | 16:5f7196ddc77b | 277 | pwmpin2 = 1; |
MarijkeZondag | 23:97a976a8f0fc | 278 | directionpin2.write(0); //rotatie omhoog |
MarijkeZondag | 22:5d956c93b3ae | 279 | ledr = 0; //red |
MarijkeZondag | 22:5d956c93b3ae | 280 | ledb = 1; |
MarijkeZondag | 22:5d956c93b3ae | 281 | ledg = 1; |
MarijkeZondag | 16:5f7196ddc77b | 282 | } |
MarijkeZondag | 22:5d956c93b3ae | 283 | else //If not higher than the threshold, motor will not turn at all |
MarijkeZondag | 16:5f7196ddc77b | 284 | { |
MarijkeZondag | 16:5f7196ddc77b | 285 | pwmpin2 = 0; |
MarijkeZondag | 22:5d956c93b3ae | 286 | ledr = 0; //white |
MarijkeZondag | 22:5d956c93b3ae | 287 | ledb = 0; |
MarijkeZondag | 22:5d956c93b3ae | 288 | ledg = 0; |
MarijkeZondag | 16:5f7196ddc77b | 289 | } |
MarijkeZondag | 23:97a976a8f0fc | 290 | // if(movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn |
MarijkeZondag | 23:97a976a8f0fc | 291 | //{ |
MarijkeZondag | 23:97a976a8f0fc | 292 | // pwmpin1 = 1; |
MarijkeZondag | 23:97a976a8f0fc | 293 | //pwmpin2 = 1; |
MarijkeZondag | 23:97a976a8f0fc | 294 | // directionpin1.write(0); //translatie achteruit |
MarijkeZondag | 23:97a976a8f0fc | 295 | // directionpin2.write(1); //rotatie omlaag |
MarijkeZondag | 23:97a976a8f0fc | 296 | |
MarijkeZondag | 23:97a976a8f0fc | 297 | //ledr = 1; //green |
MarijkeZondag | 23:97a976a8f0fc | 298 | //ledb = 1; |
MarijkeZondag | 23:97a976a8f0fc | 299 | //ledg = 0; |
MarijkeZondag | 23:97a976a8f0fc | 300 | //} |
MarijkeZondag | 23:97a976a8f0fc | 301 | //else //If not higher than the threshold, motors will not turn at all |
MarijkeZondag | 23:97a976a8f0fc | 302 | //{ |
MarijkeZondag | 23:97a976a8f0fc | 303 | // pwmpin1 = 0; |
MarijkeZondag | 23:97a976a8f0fc | 304 | // pwmpin2 = 0; |
MarijkeZondag | 23:97a976a8f0fc | 305 | |
MarijkeZondag | 23:97a976a8f0fc | 306 | // ledr = 0; //white |
MarijkeZondag | 23:97a976a8f0fc | 307 | // ledb = 0; |
MarijkeZondag | 23:97a976a8f0fc | 308 | // ledg = 0; |
MarijkeZondag | 23:97a976a8f0fc | 309 | //} |
MarijkeZondag | 22:5d956c93b3ae | 310 | |
MarijkeZondag | 23:97a976a8f0fc | 311 | break; |
MarijkeZondag | 23:97a976a8f0fc | 312 | } |
MarijkeZondag | 23:97a976a8f0fc | 313 | } |
MarijkeZondag | 23:97a976a8f0fc | 314 | |
MarijkeZondag | 23:97a976a8f0fc | 315 | |
MarijkeZondag | 23:97a976a8f0fc | 316 | int main() |
MarijkeZondag | 23:97a976a8f0fc | 317 | { |
MarijkeZondag | 23:97a976a8f0fc | 318 | pc.baud(115200); |
MarijkeZondag | 23:97a976a8f0fc | 319 | pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off. |
MarijkeZondag | 23:97a976a8f0fc | 320 | pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz |
MarijkeZondag | 23:97a976a8f0fc | 321 | |
MarijkeZondag | 23:97a976a8f0fc | 322 | emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( ¬ch1 ); //attach biquad elements to chain |
MarijkeZondag | 23:97a976a8f0fc | 323 | emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( ¬ch2 ); |
MarijkeZondag | 23:97a976a8f0fc | 324 | emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( ¬ch3 ); |
MarijkeZondag | 10:39ec51206c8b | 325 | |
MarijkeZondag | 23:97a976a8f0fc | 326 | while(true) |
MarijkeZondag | 23:97a976a8f0fc | 327 | { |
MarijkeZondag | 23:97a976a8f0fc | 328 | filter_tick.attach(&emg_filtered,T); //EMG signals filtered every T sec. |
MarijkeZondag | 23:97a976a8f0fc | 329 | MovAg_tick.attach(&MovAg,T); //Moving average calculation every T sec. |
MarijkeZondag | 23:97a976a8f0fc | 330 | Motor_tick.attach(&motor_control,T); //Test motor control |
MarijkeZondag | 23:97a976a8f0fc | 331 | |
MarijkeZondag | 23:97a976a8f0fc | 332 | // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec. |
MarijkeZondag | 23:97a976a8f0fc | 333 | |
MarijkeZondag | 23:97a976a8f0fc | 334 | button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle) |
MarijkeZondag | 23:97a976a8f0fc | 335 | wait(0.2f); |
MarijkeZondag | 23:97a976a8f0fc | 336 | button2.rise(calibrate); //Calibrate threshold for 3 muscles |
MarijkeZondag | 23:97a976a8f0fc | 337 | wait(0.2f); |
MarijkeZondag | 23:97a976a8f0fc | 338 | |
MarijkeZondag | 23:97a976a8f0fc | 339 | pc.printf("x is %i\n\r",x); |
MarijkeZondag | 23:97a976a8f0fc | 340 | pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2); |
MarijkeZondag | 23:97a976a8f0fc | 341 | pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2); |
MarijkeZondag | 23:97a976a8f0fc | 342 | //wait(2.0f); |
MarijkeZondag | 23:97a976a8f0fc | 343 | } |
vsluiter | 0:c8f15874531b | 344 | } |