Controller test2
Dependencies: QEI mbed HIDScope biquadFilter
Fork of Controllertest by
emg.h@4:e59a99c5aa08, 2016-10-25 (annotated)
- Committer:
- NahuelM
- Date:
- Tue Oct 25 10:36:50 2016 +0000
- Revision:
- 4:e59a99c5aa08
- Parent:
- 3:1d43dd4f37eb
Servo controller ge?mplementeert;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
NahuelM | 2:57523bb4e9c6 | 1 | #include "mbed.h" |
NahuelM | 2:57523bb4e9c6 | 2 | #include "BiQuad.h" |
NahuelM | 2:57523bb4e9c6 | 3 | #include "HIDScope.h" |
NahuelM | 2:57523bb4e9c6 | 4 | //Enum with states |
NahuelM | 2:57523bb4e9c6 | 5 | enum states {STATE_DEFAULT , STATE_CALIBRATION, STATE_RUN}; |
NahuelM | 2:57523bb4e9c6 | 6 | |
NahuelM | 2:57523bb4e9c6 | 7 | //Variable called 'state' |
NahuelM | 2:57523bb4e9c6 | 8 | states state = STATE_DEFAULT; |
NahuelM | 2:57523bb4e9c6 | 9 | |
NahuelM | 2:57523bb4e9c6 | 10 | //Creating two scope channels |
NahuelM | 3:1d43dd4f37eb | 11 | //HIDScope scope(2); |
NahuelM | 2:57523bb4e9c6 | 12 | |
NahuelM | 2:57523bb4e9c6 | 13 | //Notch filter |
NahuelM | 2:57523bb4e9c6 | 14 | BiQuadChain notch_50; |
NahuelM | 2:57523bb4e9c6 | 15 | BiQuad bq1( 1.00000000000, -1.60956348896, 1.00000000000, -1.40195621505, 0.74203282402); |
NahuelM | 2:57523bb4e9c6 | 16 | BiQuad bq2( 1.00000000000, -1.60724786352, 1.00000000000, -1.33646101015, 0.85967899264); |
NahuelM | 2:57523bb4e9c6 | 17 | BiQuad bq3( 1.00000000000, -1.61186693071, 1.00000000000, -1.64415455961, 0.89726621230); |
NahuelM | 2:57523bb4e9c6 | 18 | |
NahuelM | 2:57523bb4e9c6 | 19 | //High pass filter |
NahuelM | 2:57523bb4e9c6 | 20 | BiQuadChain high_pass; |
NahuelM | 2:57523bb4e9c6 | 21 | BiQuad bq4( 1.00000000000, -1.99999967822, 1.00000000000, -1.98388291862, 0.98395921205); |
NahuelM | 2:57523bb4e9c6 | 22 | BiQuad bq5( 1.00000000000, -1.99999812453, 1.00000000000, -1.99324612474, 0.99332432675); |
NahuelM | 2:57523bb4e9c6 | 23 | |
NahuelM | 2:57523bb4e9c6 | 24 | //Ticker |
NahuelM | 2:57523bb4e9c6 | 25 | Ticker emgSampleTicker; |
NahuelM | 2:57523bb4e9c6 | 26 | |
NahuelM | 2:57523bb4e9c6 | 27 | //Timeout to change state after 5 seconds |
NahuelM | 2:57523bb4e9c6 | 28 | Timeout change_state; |
NahuelM | 2:57523bb4e9c6 | 29 | |
NahuelM | 2:57523bb4e9c6 | 30 | //Timeout to change state after 10 seconds |
NahuelM | 2:57523bb4e9c6 | 31 | Timeout change_state2; |
NahuelM | 2:57523bb4e9c6 | 32 | |
NahuelM | 2:57523bb4e9c6 | 33 | //Emg input |
NahuelM | 2:57523bb4e9c6 | 34 | AnalogIn emg0( A0 ); |
NahuelM | 2:57523bb4e9c6 | 35 | AnalogIn emg1( A1 ); |
NahuelM | 2:57523bb4e9c6 | 36 | AnalogIn emg2( A2 ); |
NahuelM | 2:57523bb4e9c6 | 37 | |
NahuelM | 2:57523bb4e9c6 | 38 | bool go_emgSample; |
NahuelM | 2:57523bb4e9c6 | 39 | bool go_find_minmax; |
NahuelM | 2:57523bb4e9c6 | 40 | double emg_sample[3]; |
NahuelM | 2:57523bb4e9c6 | 41 | double emg_notch[3]; |
NahuelM | 2:57523bb4e9c6 | 42 | double emg_high_passed[3]; |
NahuelM | 2:57523bb4e9c6 | 43 | double emg_rectified; |
NahuelM | 2:57523bb4e9c6 | 44 | double min_emg[3]; |
NahuelM | 2:57523bb4e9c6 | 45 | double max_emg[3]; |
NahuelM | 2:57523bb4e9c6 | 46 | |
NahuelM | 2:57523bb4e9c6 | 47 | const int n = 200; |
NahuelM | 2:57523bb4e9c6 | 48 | int counter = 0; |
NahuelM | 2:57523bb4e9c6 | 49 | double RMSArray0[n] = {0}; |
NahuelM | 2:57523bb4e9c6 | 50 | double RMSArray1[n] = {0}; |
NahuelM | 2:57523bb4e9c6 | 51 | double RMSArray2[n] = {0}; |
NahuelM | 2:57523bb4e9c6 | 52 | double RMS0; |
NahuelM | 2:57523bb4e9c6 | 53 | double RMS1; |
NahuelM | 2:57523bb4e9c6 | 54 | double RMS2; |
NahuelM | 2:57523bb4e9c6 | 55 | double SumRMS0; |
NahuelM | 2:57523bb4e9c6 | 56 | double SumRMS1; |
NahuelM | 2:57523bb4e9c6 | 57 | double SumRMS2; |
NahuelM | 2:57523bb4e9c6 | 58 | |
NahuelM | 3:1d43dd4f37eb | 59 | double Norm_EMG_0; |
NahuelM | 3:1d43dd4f37eb | 60 | double Norm_EMG_1; |
NahuelM | 3:1d43dd4f37eb | 61 | double Norm_EMG_2; |
NahuelM | 2:57523bb4e9c6 | 62 | |
NahuelM | 2:57523bb4e9c6 | 63 | //count for emg min max |
NahuelM | 2:57523bb4e9c6 | 64 | int start_calibration = 0; |
NahuelM | 2:57523bb4e9c6 | 65 | |
NahuelM | 2:57523bb4e9c6 | 66 | void emgSample() { |
NahuelM | 2:57523bb4e9c6 | 67 | go_emgSample = true; |
NahuelM | 2:57523bb4e9c6 | 68 | } |
NahuelM | 2:57523bb4e9c6 | 69 | |
NahuelM | 2:57523bb4e9c6 | 70 | void calibrate() { |
NahuelM | 2:57523bb4e9c6 | 71 | state = STATE_CALIBRATION; |
NahuelM | 2:57523bb4e9c6 | 72 | } |
NahuelM | 2:57523bb4e9c6 | 73 | |
NahuelM | 2:57523bb4e9c6 | 74 | void run() { |
NahuelM | 2:57523bb4e9c6 | 75 | state = STATE_RUN; |
NahuelM | 2:57523bb4e9c6 | 76 | } |
NahuelM | 2:57523bb4e9c6 | 77 | |
NahuelM | 2:57523bb4e9c6 | 78 | void EMG_filter(); |
NahuelM | 2:57523bb4e9c6 | 79 | /* |
NahuelM | 2:57523bb4e9c6 | 80 | int main() { |
NahuelM | 2:57523bb4e9c6 | 81 | //combine biquads in biquad chains for notch/high- low-pass filters |
NahuelM | 2:57523bb4e9c6 | 82 | notch_50.add( &bq1 ).add( &bq2 ).add( &bq3 ); |
NahuelM | 2:57523bb4e9c6 | 83 | high_pass.add( &bq4 ).add( &bq5 ); |
NahuelM | 2:57523bb4e9c6 | 84 | |
NahuelM | 2:57523bb4e9c6 | 85 | change_state.attach( &calibrate,5); |
NahuelM | 2:57523bb4e9c6 | 86 | change_state2.attach( &run,10); |
NahuelM | 2:57523bb4e9c6 | 87 | emgSampleTicker.attach( &emgSample, 0.002); |
NahuelM | 2:57523bb4e9c6 | 88 | while( true ){ |
NahuelM | 2:57523bb4e9c6 | 89 | if(go_emgSample == true){ |
NahuelM | 2:57523bb4e9c6 | 90 | EMG_filter(); |
NahuelM | 2:57523bb4e9c6 | 91 | } |
NahuelM | 2:57523bb4e9c6 | 92 | } |
NahuelM | 2:57523bb4e9c6 | 93 | } |
NahuelM | 2:57523bb4e9c6 | 94 | */ |
NahuelM | 2:57523bb4e9c6 | 95 | |
NahuelM | 2:57523bb4e9c6 | 96 | void EMG_filter() { |
NahuelM | 2:57523bb4e9c6 | 97 | if(go_emgSample == true){ |
NahuelM | 2:57523bb4e9c6 | 98 | //read the emg signal |
NahuelM | 2:57523bb4e9c6 | 99 | emg_sample[0] = emg0.read(); |
NahuelM | 2:57523bb4e9c6 | 100 | emg_sample[1] = emg1.read(); |
NahuelM | 2:57523bb4e9c6 | 101 | emg_sample[2] = emg2.read(); |
NahuelM | 2:57523bb4e9c6 | 102 | |
NahuelM | 2:57523bb4e9c6 | 103 | for (int i = 0; i < 3; i++){ |
NahuelM | 2:57523bb4e9c6 | 104 | //filter out the 50Hz components with a notch filter |
NahuelM | 2:57523bb4e9c6 | 105 | //emg_notch[i] = notch_50.step(emg_sample[i]); |
NahuelM | 2:57523bb4e9c6 | 106 | |
NahuelM | 2:57523bb4e9c6 | 107 | //high pass the signal (removing motion artifacts and offset) |
NahuelM | 2:57523bb4e9c6 | 108 | emg_high_passed[i] = high_pass.step(emg_sample[i]); |
NahuelM | 2:57523bb4e9c6 | 109 | } |
NahuelM | 2:57523bb4e9c6 | 110 | |
NahuelM | 2:57523bb4e9c6 | 111 | //Calculating RMS |
NahuelM | 2:57523bb4e9c6 | 112 | SumRMS0 -= pow(RMSArray0[counter],2); |
NahuelM | 2:57523bb4e9c6 | 113 | SumRMS1 -= pow(RMSArray1[counter],2); |
NahuelM | 2:57523bb4e9c6 | 114 | SumRMS2 -= pow(RMSArray2[counter],2); |
NahuelM | 2:57523bb4e9c6 | 115 | |
NahuelM | 2:57523bb4e9c6 | 116 | RMSArray0[counter] = emg_high_passed[0]; |
NahuelM | 2:57523bb4e9c6 | 117 | RMSArray1[counter] = emg_high_passed[1]; |
NahuelM | 2:57523bb4e9c6 | 118 | RMSArray2[counter] = emg_high_passed[2]; |
NahuelM | 2:57523bb4e9c6 | 119 | |
NahuelM | 2:57523bb4e9c6 | 120 | SumRMS0 += pow(RMSArray0[counter],2); |
NahuelM | 2:57523bb4e9c6 | 121 | SumRMS1 += pow(RMSArray1[counter],2); |
NahuelM | 2:57523bb4e9c6 | 122 | SumRMS2 += pow(RMSArray2[counter],2); |
NahuelM | 2:57523bb4e9c6 | 123 | |
NahuelM | 2:57523bb4e9c6 | 124 | counter++; |
NahuelM | 2:57523bb4e9c6 | 125 | if (counter == n){ |
NahuelM | 2:57523bb4e9c6 | 126 | counter = 0; |
NahuelM | 2:57523bb4e9c6 | 127 | } |
NahuelM | 2:57523bb4e9c6 | 128 | |
NahuelM | 2:57523bb4e9c6 | 129 | RMS0 = sqrt(SumRMS0/n); |
NahuelM | 2:57523bb4e9c6 | 130 | RMS1 = sqrt(SumRMS1/n); |
NahuelM | 2:57523bb4e9c6 | 131 | RMS2 = sqrt(SumRMS2/n); |
NahuelM | 2:57523bb4e9c6 | 132 | |
NahuelM | 2:57523bb4e9c6 | 133 | //Calculating min value and max value of emg signal |
NahuelM | 2:57523bb4e9c6 | 134 | if(state == STATE_CALIBRATION) |
NahuelM | 2:57523bb4e9c6 | 135 | { |
NahuelM | 2:57523bb4e9c6 | 136 | if (start_calibration == 0) { |
NahuelM | 2:57523bb4e9c6 | 137 | min_emg[0] = RMS0; |
NahuelM | 2:57523bb4e9c6 | 138 | max_emg[0] = RMS0; |
NahuelM | 2:57523bb4e9c6 | 139 | min_emg[1] = RMS1; |
NahuelM | 2:57523bb4e9c6 | 140 | max_emg[1] = RMS1; |
NahuelM | 2:57523bb4e9c6 | 141 | min_emg[2] = RMS2; |
NahuelM | 2:57523bb4e9c6 | 142 | max_emg[2] = RMS2; |
NahuelM | 2:57523bb4e9c6 | 143 | start_calibration++; |
NahuelM | 2:57523bb4e9c6 | 144 | } |
NahuelM | 2:57523bb4e9c6 | 145 | else { |
NahuelM | 2:57523bb4e9c6 | 146 | //finding min and max of emg0 |
NahuelM | 2:57523bb4e9c6 | 147 | if (RMS0 < min_emg[0]) { |
NahuelM | 2:57523bb4e9c6 | 148 | min_emg[0] = RMS0; |
NahuelM | 2:57523bb4e9c6 | 149 | } |
NahuelM | 2:57523bb4e9c6 | 150 | else if (RMS0 > max_emg[0]) { |
NahuelM | 2:57523bb4e9c6 | 151 | max_emg[0] = RMS0; |
NahuelM | 2:57523bb4e9c6 | 152 | } |
NahuelM | 2:57523bb4e9c6 | 153 | |
NahuelM | 2:57523bb4e9c6 | 154 | //finding min and max of emg1 |
NahuelM | 2:57523bb4e9c6 | 155 | if (RMS1 < min_emg[1]) { |
NahuelM | 2:57523bb4e9c6 | 156 | min_emg[1] = RMS1; |
NahuelM | 2:57523bb4e9c6 | 157 | } |
NahuelM | 2:57523bb4e9c6 | 158 | else if (RMS1 > max_emg[1]) { |
NahuelM | 2:57523bb4e9c6 | 159 | max_emg[1] = RMS1; |
NahuelM | 2:57523bb4e9c6 | 160 | } |
NahuelM | 2:57523bb4e9c6 | 161 | |
NahuelM | 2:57523bb4e9c6 | 162 | //finding min and max of emg2 |
NahuelM | 2:57523bb4e9c6 | 163 | if (RMS2 < min_emg[2]) { |
NahuelM | 2:57523bb4e9c6 | 164 | min_emg[2] = RMS2; |
NahuelM | 2:57523bb4e9c6 | 165 | } |
NahuelM | 2:57523bb4e9c6 | 166 | else if (RMS2 > max_emg[2]) { |
NahuelM | 2:57523bb4e9c6 | 167 | max_emg[2] = RMS2; |
NahuelM | 2:57523bb4e9c6 | 168 | } |
NahuelM | 2:57523bb4e9c6 | 169 | } |
NahuelM | 2:57523bb4e9c6 | 170 | } |
NahuelM | 2:57523bb4e9c6 | 171 | |
NahuelM | 2:57523bb4e9c6 | 172 | //calculating input_forces for controller |
NahuelM | 3:1d43dd4f37eb | 173 | Norm_EMG_0 = (RMS0 - min_emg[0])/(max_emg[0]-min_emg[0]); |
NahuelM | 3:1d43dd4f37eb | 174 | Norm_EMG_1 = (RMS1 - min_emg[1])/(max_emg[1]-min_emg[1]); |
NahuelM | 3:1d43dd4f37eb | 175 | Norm_EMG_2 = (RMS2 - min_emg[2])/(max_emg[2]-min_emg[2]); |
NahuelM | 2:57523bb4e9c6 | 176 | |
NahuelM | 2:57523bb4e9c6 | 177 | //Send scope data |
NahuelM | 3:1d43dd4f37eb | 178 | // scope.set(0,emg_sample[0]); |
NahuelM | 3:1d43dd4f37eb | 179 | //scope.set(1,Norm_EMG_0); |
NahuelM | 2:57523bb4e9c6 | 180 | //scope.set(2,input_force1); |
NahuelM | 2:57523bb4e9c6 | 181 | //scope.set(3,input_force2); |
NahuelM | 3:1d43dd4f37eb | 182 | //scope.send(); |
NahuelM | 2:57523bb4e9c6 | 183 | |
NahuelM | 2:57523bb4e9c6 | 184 | go_emgSample = false; |
NahuelM | 2:57523bb4e9c6 | 185 | } |
NahuelM | 2:57523bb4e9c6 | 186 | } |
NahuelM | 2:57523bb4e9c6 | 187 |