Nahuel Manterola / Mbed 2 deprecated Controllertest2

Controller test2

Dependencies:   QEI mbed HIDScope biquadFilter

Fork of Controllertest by Nahuel Manterola

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?

UserRevisionLine numberNew 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