Chris Verhoeven / Mbed 2 deprecated Filter

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Fork of Filter by Rick Koetsier

Calibrationscript.cpp@8:237b1e262ebd, 2017-11-01 (annotated)

Committer:
aluminium
Date:
Wed Nov 01 14:54:24 2017 +0000
Revision:
8:237b1e262ebd
Parent:
7:7c6a9bb2d30e
Child:
9:76bc987121d3
Lilnks en rechts werkt !

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DAkoetsier 2:8f6fca2179f2 1 #include "mbed.h"
DAkoetsier 2:8f6fca2179f2 2 #include "HIDScope.h" //require the HIDScope library
DAkoetsier 2:8f6fca2179f2 3 #include "MODSERIAL.h"
DAkoetsier 2:8f6fca2179f2 4 #include "BiQuad.h"
aluminium 5:7c8176cdaa1c 5 # include "math.h"
aluminium 7:7c6a9bb2d30e 6 // Rechterarm
DAkoetsier 2:8f6fca2179f2 7 // DEFINING
DAkoetsier 2:8f6fca2179f2 8 //Define Inputs
aluminium 7:7c6a9bb2d30e 9 //rechterarm
aluminium 7:7c6a9bb2d30e 10 AnalogIn emg_r(A0); //analog of emg_r input
aluminium 7:7c6a9bb2d30e 11 //linkerarm
aluminium 7:7c6a9bb2d30e 12 AnalogIn emg_l(A1);
aluminium 7:7c6a9bb2d30e 13
DAkoetsier 2:8f6fca2179f2 14 InterruptIn button1(PTA4); //test button for starting motor 1
DAkoetsier 2:8f6fca2179f2 15 InterruptIn button2(SW2); //FOR DEBUGGING
DAkoetsier 2:8f6fca2179f2 16
DAkoetsier 2:8f6fca2179f2 17 //Define Outputs
DAkoetsier 2:8f6fca2179f2 18 DigitalOut led1(LED_RED);
DAkoetsier 2:8f6fca2179f2 19 DigitalOut led2(LED_BLUE);
DAkoetsier 2:8f6fca2179f2 20 DigitalOut led3(LED_GREEN); //FOR DEBUGGING
DAkoetsier 2:8f6fca2179f2 21 MODSERIAL pc(USBTX,USBRX);
DAkoetsier 2:8f6fca2179f2 22
DAkoetsier 2:8f6fca2179f2 23
DAkoetsier 2:8f6fca2179f2 24 //Define Tickers
aluminium 7:7c6a9bb2d30e 25 //rechterarm
aluminium 7:7c6a9bb2d30e 26 Ticker LED_timer_r; // Write the LED
aluminium 5:7c8176cdaa1c 27
aluminium 7:7c6a9bb2d30e 28 //linkerarm
aluminium 7:7c6a9bb2d30e 29 Ticker LED_timer_l; // Write the LED
DAkoetsier 2:8f6fca2179f2 30
DAkoetsier 2:8f6fca2179f2 31 //Define HIDscope
aluminium 4:285fb7d84088 32 //HIDScope scope(2); //instantize a 2-channel HIDScope object
DAkoetsier 2:8f6fca2179f2 33
DAkoetsier 2:8f6fca2179f2 34
DAkoetsier 2:8f6fca2179f2 35 //Define filters and define the floats which contains the values.
DAkoetsier 2:8f6fca2179f2 36 BiQuadChain bqc;
aluminium 8:237b1e262ebd 37 BiQuad bq2_high(0.875182, -1.750364, 0.87518, -1.73472, 0.766004);
aluminium 8:237b1e262ebd 38 BiQuad bq3_notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780);
aluminium 8:237b1e262ebd 39 BiQuad bq1_low(3.65747e-2, 7.31495e-2, 3.65747e-2, -1.390892, 0.537191);
DAkoetsier 2:8f6fca2179f2 40
aluminium 7:7c6a9bb2d30e 41
aluminium 7:7c6a9bb2d30e 42
aluminium 7:7c6a9bb2d30e 43 // Rechterarm
aluminium 7:7c6a9bb2d30e 44 double emgFiltered_r;
aluminium 7:7c6a9bb2d30e 45 double filteredAbs_r;
aluminium 7:7c6a9bb2d30e 46 double emg_value_r;
aluminium 7:7c6a9bb2d30e 47 double onoffsignal_r;
aluminium 7:7c6a9bb2d30e 48 bool check_calibration_r=0;
aluminium 7:7c6a9bb2d30e 49 double avg_emg_r;
aluminium 7:7c6a9bb2d30e 50 bool rechterarm_positief_r = false;
aluminium 7:7c6a9bb2d30e 51 bool rechterarm_negatief_r = false;
DAkoetsier 2:8f6fca2179f2 52
aluminium 7:7c6a9bb2d30e 53 //Linkerarm
aluminium 7:7c6a9bb2d30e 54 double emgFiltered_l;
aluminium 7:7c6a9bb2d30e 55 double filteredAbs_l;
aluminium 7:7c6a9bb2d30e 56 double emg_value_l;
aluminium 7:7c6a9bb2d30e 57 double onoffsignal_l;
aluminium 7:7c6a9bb2d30e 58 bool check_calibration_l=0;
aluminium 7:7c6a9bb2d30e 59 double avg_emg_l;
aluminium 7:7c6a9bb2d30e 60 bool linkerarm_positief_l = false;
aluminium 7:7c6a9bb2d30e 61 bool linkerarm_negatief_l = false;
aluminium 7:7c6a9bb2d30e 62
aluminium 7:7c6a9bb2d30e 63 // FUNCTIONS
aluminium 7:7c6a9bb2d30e 64
aluminium 7:7c6a9bb2d30e 65 //Rechterarm
DAkoetsier 2:8f6fca2179f2 66 //function for filtering
DAkoetsier 2:8f6fca2179f2 67
aluminium 7:7c6a9bb2d30e 68 void filter_r(){
aluminium 7:7c6a9bb2d30e 69 if(check_calibration_r==1){
aluminium 7:7c6a9bb2d30e 70
aluminium 7:7c6a9bb2d30e 71
aluminium 7:7c6a9bb2d30e 72
aluminium 7:7c6a9bb2d30e 73
aluminium 7:7c6a9bb2d30e 74 emg_value_r = emg_r.read();
aluminium 7:7c6a9bb2d30e 75 emgFiltered_r = bqc.step( emg_value_r );
aluminium 7:7c6a9bb2d30e 76 filteredAbs_r = abs( emgFiltered_r );
aluminium 7:7c6a9bb2d30e 77 if (avg_emg_r != 0){
aluminium 7:7c6a9bb2d30e 78 onoffsignal_r=filteredAbs_r/avg_emg_r; //divide the emg_r signal by the max emg_r to calibrate the signal per person
aluminium 7:7c6a9bb2d30e 79 }
aluminium 7:7c6a9bb2d30e 80 // scope.set(0,emg_value_r); //set emg_r signal to scope in channel 1
aluminium 7:7c6a9bb2d30e 81 //scope.set(1,onoffsignal_r); //set filtered signal to scope in channel 2
aluminium 7:7c6a9bb2d30e 82 // scope.send(); //send the signals to the scope
aluminium 7:7c6a9bb2d30e 83 // pc.printf("emg_r signal %f, filtered signal %f \n",emg_value_r,onoffsignal_r);
aluminium 7:7c6a9bb2d30e 84 }
aluminium 7:7c6a9bb2d30e 85 }
aluminium 7:7c6a9bb2d30e 86
aluminium 7:7c6a9bb2d30e 87 //Linkerarm
aluminium 7:7c6a9bb2d30e 88 //function for filtering
aluminium 7:7c6a9bb2d30e 89
aluminium 7:7c6a9bb2d30e 90 void filter_l(){
aluminium 7:7c6a9bb2d30e 91 if(check_calibration_l==1){
DAkoetsier 2:8f6fca2179f2 92
aluminium 6:b9a84c1cb4f9 93
aluminium 6:b9a84c1cb4f9 94
aluminium 6:b9a84c1cb4f9 95
aluminium 7:7c6a9bb2d30e 96 emg_value_l = emg_l.read();
aluminium 7:7c6a9bb2d30e 97 emgFiltered_l = bqc.step( emg_value_l );
aluminium 7:7c6a9bb2d30e 98 filteredAbs_l = abs( emgFiltered_l );
aluminium 7:7c6a9bb2d30e 99 if (avg_emg_l != 0){
aluminium 8:237b1e262ebd 100 onoffsignal_l=filteredAbs_l/avg_emg_l; //divide the emg_r signal by the avg_emg_l wat staat voor avg emg in gespannen toestand
aluminium 6:b9a84c1cb4f9 101 }
aluminium 7:7c6a9bb2d30e 102
DAkoetsier 2:8f6fca2179f2 103 }
DAkoetsier 2:8f6fca2179f2 104 }
DAkoetsier 2:8f6fca2179f2 105
aluminium 7:7c6a9bb2d30e 106
aluminium 7:7c6a9bb2d30e 107 //Rechterarm
DAkoetsier 2:8f6fca2179f2 108 //function to check the threshold
aluminium 7:7c6a9bb2d30e 109 void check_emg_r(){
aluminium 7:7c6a9bb2d30e 110 double filteredAbs_temp_r;
aluminium 6:b9a84c1cb4f9 111
aluminium 8:237b1e262ebd 112 if((check_calibration_l==1) &&(check_calibration_r==1)){
aluminium 8:237b1e262ebd 113 for( int i = 0; i<500;i++){
aluminium 7:7c6a9bb2d30e 114 filter_r();
aluminium 7:7c6a9bb2d30e 115 filteredAbs_temp_r = filteredAbs_temp_r + onoffsignal_r;
aluminium 6:b9a84c1cb4f9 116 wait(0.0004);
aluminium 6:b9a84c1cb4f9 117 }
aluminium 8:237b1e262ebd 118 filteredAbs_temp_r = filteredAbs_temp_r/500;
aluminium 8:237b1e262ebd 119 if(filteredAbs_temp_r<=0.3){ //if signal is lower then 0.5 the blue light goes on
aluminium 6:b9a84c1cb4f9 120 led1.write(1); //led 1 is rood en uit
aluminium 8:237b1e262ebd 121
aluminium 7:7c6a9bb2d30e 122 rechterarm_positief_r = false;
aluminium 7:7c6a9bb2d30e 123 rechterarm_negatief_r = true;
aluminium 7:7c6a9bb2d30e 124
DAkoetsier 2:8f6fca2179f2 125 }
aluminium 8:237b1e262ebd 126 else if(filteredAbs_temp_r > 0.3){ //if signal does not pass threshold value, blue light goes on
DAkoetsier 2:8f6fca2179f2 127 led1.write(0);
aluminium 8:237b1e262ebd 128
aluminium 7:7c6a9bb2d30e 129 rechterarm_negatief_r = false;
aluminium 7:7c6a9bb2d30e 130 rechterarm_positief_r = true;
DAkoetsier 2:8f6fca2179f2 131 }
aluminium 7:7c6a9bb2d30e 132
DAkoetsier 2:8f6fca2179f2 133 }
DAkoetsier 2:8f6fca2179f2 134 }
DAkoetsier 2:8f6fca2179f2 135
aluminium 7:7c6a9bb2d30e 136 //Linkerarm
aluminium 7:7c6a9bb2d30e 137 //function to check the threshold
aluminium 7:7c6a9bb2d30e 138 void check_emg_l(){
aluminium 7:7c6a9bb2d30e 139 double filteredAbs_temp_l;
aluminium 7:7c6a9bb2d30e 140
aluminium 8:237b1e262ebd 141 if((check_calibration_l)==1 &&(check_calibration_r==1) ){
aluminium 8:237b1e262ebd 142 for( int i = 0; i<500;i++){
aluminium 7:7c6a9bb2d30e 143 filter_l();
aluminium 7:7c6a9bb2d30e 144 filteredAbs_temp_l = filteredAbs_temp_l + onoffsignal_l;
aluminium 7:7c6a9bb2d30e 145 wait(0.0004);
aluminium 7:7c6a9bb2d30e 146 }
aluminium 8:237b1e262ebd 147 filteredAbs_temp_l = filteredAbs_temp_l/500;
aluminium 8:237b1e262ebd 148 if(filteredAbs_temp_l<=0.3){ //if signal is lower then 0.5 the blue light goes on
aluminium 8:237b1e262ebd 149 // led1.write(1); //led 1 is rood en uit
aluminium 8:237b1e262ebd 150 led2.write(1); //led 2 is blauw en aan
aluminium 7:7c6a9bb2d30e 151 linkerarm_positief_l = false;
aluminium 7:7c6a9bb2d30e 152 linkerarm_negatief_l = true;
aluminium 7:7c6a9bb2d30e 153
aluminium 7:7c6a9bb2d30e 154 }
aluminium 8:237b1e262ebd 155 else if(filteredAbs_temp_l > 0.3){ //if signal does not pass threshold value, blue light goes on
aluminium 8:237b1e262ebd 156 // led1.write(0);
aluminium 8:237b1e262ebd 157 led2.write(0);
aluminium 7:7c6a9bb2d30e 158 linkerarm_negatief_l = false;
aluminium 7:7c6a9bb2d30e 159 linkerarm_positief_l = true;
aluminium 7:7c6a9bb2d30e 160 }
aluminium 7:7c6a9bb2d30e 161
aluminium 7:7c6a9bb2d30e 162 }
aluminium 7:7c6a9bb2d30e 163 }
aluminium 7:7c6a9bb2d30e 164
aluminium 7:7c6a9bb2d30e 165
aluminium 7:7c6a9bb2d30e 166 //rechterarm
DAkoetsier 2:8f6fca2179f2 167 //function to calibrate
aluminium 7:7c6a9bb2d30e 168 int calibration_r(){
aluminium 7:7c6a9bb2d30e 169
aluminium 5:7c8176cdaa1c 170
aluminium 5:7c8176cdaa1c 171 // if(button1.read()==false){
aluminium 5:7c8176cdaa1c 172
aluminium 5:7c8176cdaa1c 173
aluminium 5:7c8176cdaa1c 174
DAkoetsier 2:8f6fca2179f2 175 led3.write(0);
aluminium 5:7c8176cdaa1c 176
aluminium 7:7c6a9bb2d30e 177 double signal_verzameling_r = 0;
aluminium 5:7c8176cdaa1c 178 for(int n =0; n<5000;n++){
aluminium 7:7c6a9bb2d30e 179 filter_r();
aluminium 5:7c8176cdaa1c 180 //read for 5000 samples as calibration
aluminium 7:7c6a9bb2d30e 181 emg_value_r = emg_r.read();
aluminium 7:7c6a9bb2d30e 182 emgFiltered_r = bqc.step( emg_value_r );
aluminium 7:7c6a9bb2d30e 183 filteredAbs_r = abs(emgFiltered_r);
aluminium 5:7c8176cdaa1c 184
aluminium 5:7c8176cdaa1c 185
aluminium 7:7c6a9bb2d30e 186 // signal_verzameling[n]= filteredAbs_r;
aluminium 7:7c6a9bb2d30e 187 signal_verzameling_r = signal_verzameling_r + filteredAbs_r ;
aluminium 6:b9a84c1cb4f9 188 wait(0.0004);
aluminium 6:b9a84c1cb4f9 189
aluminium 5:7c8176cdaa1c 190 if (n == 4999){
aluminium 7:7c6a9bb2d30e 191 avg_emg_r = signal_verzameling_r / n;
aluminium 7:7c6a9bb2d30e 192
aluminium 5:7c8176cdaa1c 193 }
aluminium 5:7c8176cdaa1c 194 }
aluminium 5:7c8176cdaa1c 195
aluminium 5:7c8176cdaa1c 196 led3.write(1);
aluminium 8:237b1e262ebd 197 pc.printf("calibratie rechts compleet\n\r");
aluminium 5:7c8176cdaa1c 198 // double lengte_array = sizeof(signal_verzameling);
aluminium 5:7c8176cdaa1c 199 // pc.printf("lengte_array = %f\n\r",lengte_array);
aluminium 4:285fb7d84088 200
aluminium 5:7c8176cdaa1c 201 // for(int i = 0; i < lengte_array; i++){
aluminium 5:7c8176cdaa1c 202
aluminium 5:7c8176cdaa1c 203
aluminium 5:7c8176cdaa1c 204 // sum_array = sum_array + signal_verzameling[i] ;
aluminium 5:7c8176cdaa1c 205 // }
aluminium 7:7c6a9bb2d30e 206 //avg_emg_r = sum_array / lengte_array;
aluminium 7:7c6a9bb2d30e 207 // pc.printf("avg_emg_r = %f\n\r",avg_emg_r);
aluminium 4:285fb7d84088 208
aluminium 4:285fb7d84088 209
aluminium 4:285fb7d84088 210
aluminium 4:285fb7d84088 211
aluminium 7:7c6a9bb2d30e 212 check_calibration_r=1;
aluminium 8:237b1e262ebd 213
aluminium 7:7c6a9bb2d30e 214 // }
aluminium 7:7c6a9bb2d30e 215 return 0;
aluminium 7:7c6a9bb2d30e 216 }
aluminium 7:7c6a9bb2d30e 217
aluminium 7:7c6a9bb2d30e 218 //linkeram
aluminium 7:7c6a9bb2d30e 219 //function to calibrate
aluminium 7:7c6a9bb2d30e 220 int calibration_l(){
aluminium 7:7c6a9bb2d30e 221
aluminium 7:7c6a9bb2d30e 222
aluminium 7:7c6a9bb2d30e 223 // if(button1.read()==false){
aluminium 7:7c6a9bb2d30e 224
aluminium 7:7c6a9bb2d30e 225
aluminium 7:7c6a9bb2d30e 226
aluminium 7:7c6a9bb2d30e 227 led3.write(0);
aluminium 7:7c6a9bb2d30e 228
aluminium 7:7c6a9bb2d30e 229 double signal_verzameling_l= 0;
aluminium 7:7c6a9bb2d30e 230 for(int n =0; n<5000;n++){
aluminium 7:7c6a9bb2d30e 231 filter_l();
aluminium 7:7c6a9bb2d30e 232 //read for 5000 samples as calibration
aluminium 7:7c6a9bb2d30e 233 emg_value_l = emg_l.read();
aluminium 7:7c6a9bb2d30e 234 emgFiltered_l = bqc.step( emg_value_l );
aluminium 7:7c6a9bb2d30e 235 filteredAbs_l = abs(emgFiltered_l);
aluminium 7:7c6a9bb2d30e 236
aluminium 7:7c6a9bb2d30e 237
aluminium 7:7c6a9bb2d30e 238 // signal_verzameling[n]= filteredAbs_r;
aluminium 7:7c6a9bb2d30e 239 signal_verzameling_l = signal_verzameling_l + filteredAbs_l ;
aluminium 7:7c6a9bb2d30e 240 wait(0.0004);
aluminium 7:7c6a9bb2d30e 241
aluminium 7:7c6a9bb2d30e 242 if (n == 4999){
aluminium 7:7c6a9bb2d30e 243 avg_emg_l = signal_verzameling_l / n;
aluminium 7:7c6a9bb2d30e 244
aluminium 7:7c6a9bb2d30e 245 }
aluminium 7:7c6a9bb2d30e 246 }
aluminium 7:7c6a9bb2d30e 247
aluminium 7:7c6a9bb2d30e 248
aluminium 8:237b1e262ebd 249
aluminium 7:7c6a9bb2d30e 250
aluminium 7:7c6a9bb2d30e 251
aluminium 7:7c6a9bb2d30e 252 // sum_array = sum_array + signal_verzameling[i] ;
aluminium 7:7c6a9bb2d30e 253 // }
aluminium 7:7c6a9bb2d30e 254 //avg_emg_r = sum_array / lengte_array;
aluminium 7:7c6a9bb2d30e 255 // pc.printf("avg_emg_r = %f\n\r",avg_emg_r);
aluminium 7:7c6a9bb2d30e 256
aluminium 7:7c6a9bb2d30e 257
aluminium 7:7c6a9bb2d30e 258
aluminium 7:7c6a9bb2d30e 259
aluminium 7:7c6a9bb2d30e 260 check_calibration_l=1;
DAkoetsier 2:8f6fca2179f2 261 led3.write(1);
aluminium 8:237b1e262ebd 262 pc.printf("calibratie links compleet\n\r");
aluminium 5:7c8176cdaa1c 263 // }
aluminium 5:7c8176cdaa1c 264 return 0;
DAkoetsier 2:8f6fca2179f2 265 }
DAkoetsier 2:8f6fca2179f2 266
DAkoetsier 2:8f6fca2179f2 267 // MAIN
DAkoetsier 2:8f6fca2179f2 268
DAkoetsier 2:8f6fca2179f2 269 int main(){
aluminium 5:7c8176cdaa1c 270 pc.baud(115200);
DAkoetsier 2:8f6fca2179f2 271
aluminium 5:7c8176cdaa1c 272 pc.printf("Lampjes zijn langs geweest");
DAkoetsier 2:8f6fca2179f2 273 led1.write(1);
DAkoetsier 2:8f6fca2179f2 274 led2.write(1);
DAkoetsier 2:8f6fca2179f2 275 led3.write(1);
DAkoetsier 2:8f6fca2179f2 276
aluminium 4:285fb7d84088 277
aluminium 4:285fb7d84088 278
DAkoetsier 2:8f6fca2179f2 279 bqc.add( &bq1_low ).add( &bq2_high ).add( &bq3_notch );
DAkoetsier 2:8f6fca2179f2 280
aluminium 7:7c6a9bb2d30e 281
aluminium 7:7c6a9bb2d30e 282 LED_timer_r.attach(&check_emg_r, 0.1); //continously execute the motor controller
aluminium 7:7c6a9bb2d30e 283 LED_timer_l.attach(&check_emg_l, 0.1); //continously execute the motor controller
aluminium 7:7c6a9bb2d30e 284
DAkoetsier 2:8f6fca2179f2 285
aluminium 7:7c6a9bb2d30e 286 calibration_r();
aluminium 7:7c6a9bb2d30e 287 calibration_l();
DAkoetsier 2:8f6fca2179f2 288
DAkoetsier 2:8f6fca2179f2 289 while(1){ //while loop to keep system going
DAkoetsier 2:8f6fca2179f2 290
DAkoetsier 2:8f6fca2179f2 291 }
DAkoetsier 2:8f6fca2179f2 292 }