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@7:7c6a9bb2d30e, 2017-11-01 (annotated)

Committer:
aluminium
Date:
Wed Nov 01 13:36:54 2017 +0000
Revision:
7:7c6a9bb2d30e
Parent:
6:b9a84c1cb4f9
Child:
8:237b1e262ebd
Voor linker en rechterarm

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 6:b9a84c1cb4f9 37 BiQuad bq1_low(0.00182, 0.0036417, 0.0018208, -1.8764998, 0.883783);
aluminium 6:b9a84c1cb4f9 38 BiQuad bq2_high( 0.973868, -1.947737, 0.97386, -1.947737, 0.948429);
DAkoetsier 2:8f6fca2179f2 39 BiQuad bq3_notch(0.91859, -1.82269, 0.91859, -1.82269, 0.83718);
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 7:7c6a9bb2d30e 100 onoffsignal_l=filteredAbs_l/avg_emg_l; //divide the emg_r signal by the max emg_r to calibrate the signal per person
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 7:7c6a9bb2d30e 112 if(check_calibration_r==1){
aluminium 6:b9a84c1cb4f9 113 for( int i = 0; i<1000;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 7:7c6a9bb2d30e 118 filteredAbs_temp_r = filteredAbs_temp_r/1000;
aluminium 7:7c6a9bb2d30e 119 if(filteredAbs_temp_r<=0.55){ //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 6:b9a84c1cb4f9 121 led2.write(0); //led 2 is blauw en aan
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 7:7c6a9bb2d30e 126 else if(filteredAbs_temp_r > 0.55){ //if signal does not pass threshold value, blue light goes on
DAkoetsier 2:8f6fca2179f2 127 led1.write(0);
DAkoetsier 2:8f6fca2179f2 128 led2.write(1);
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 7:7c6a9bb2d30e 141 if(check_calibration_l==1){
aluminium 7:7c6a9bb2d30e 142 for( int i = 0; i<1000;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 7:7c6a9bb2d30e 147 filteredAbs_temp_l = filteredAbs_temp_l/1000;
aluminium 7:7c6a9bb2d30e 148 if(filteredAbs_temp_l<=0.55){ //if signal is lower then 0.5 the blue light goes on
aluminium 7:7c6a9bb2d30e 149 led1.write(1); //led 1 is rood en uit
aluminium 7:7c6a9bb2d30e 150 led2.write(0); //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 7:7c6a9bb2d30e 155 else if(filteredAbs_temp_l > 0.55){ //if signal does not pass threshold value, blue light goes on
aluminium 7:7c6a9bb2d30e 156 led1.write(0);
aluminium 7:7c6a9bb2d30e 157 led2.write(1);
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 5:7c8176cdaa1c 197 // double lengte_array = sizeof(signal_verzameling);
aluminium 5:7c8176cdaa1c 198 // pc.printf("lengte_array = %f\n\r",lengte_array);
aluminium 4:285fb7d84088 199
aluminium 5:7c8176cdaa1c 200 // for(int i = 0; i < lengte_array; i++){
aluminium 5:7c8176cdaa1c 201
aluminium 5:7c8176cdaa1c 202
aluminium 5:7c8176cdaa1c 203 // sum_array = sum_array + signal_verzameling[i] ;
aluminium 5:7c8176cdaa1c 204 // }
aluminium 7:7c6a9bb2d30e 205 //avg_emg_r = sum_array / lengte_array;
aluminium 7:7c6a9bb2d30e 206 // pc.printf("avg_emg_r = %f\n\r",avg_emg_r);
aluminium 4:285fb7d84088 207
aluminium 4:285fb7d84088 208
aluminium 4:285fb7d84088 209
aluminium 4:285fb7d84088 210
aluminium 7:7c6a9bb2d30e 211 check_calibration_r=1;
aluminium 7:7c6a9bb2d30e 212 led3.write(1);
aluminium 7:7c6a9bb2d30e 213 // }
aluminium 7:7c6a9bb2d30e 214 return 0;
aluminium 7:7c6a9bb2d30e 215 }
aluminium 7:7c6a9bb2d30e 216
aluminium 7:7c6a9bb2d30e 217 //linkeram
aluminium 7:7c6a9bb2d30e 218 //function to calibrate
aluminium 7:7c6a9bb2d30e 219 int calibration_l(){
aluminium 7:7c6a9bb2d30e 220
aluminium 7:7c6a9bb2d30e 221
aluminium 7:7c6a9bb2d30e 222 // if(button1.read()==false){
aluminium 7:7c6a9bb2d30e 223
aluminium 7:7c6a9bb2d30e 224
aluminium 7:7c6a9bb2d30e 225
aluminium 7:7c6a9bb2d30e 226 led3.write(0);
aluminium 7:7c6a9bb2d30e 227
aluminium 7:7c6a9bb2d30e 228 double signal_verzameling_l= 0;
aluminium 7:7c6a9bb2d30e 229 for(int n =0; n<5000;n++){
aluminium 7:7c6a9bb2d30e 230 filter_l();
aluminium 7:7c6a9bb2d30e 231 //read for 5000 samples as calibration
aluminium 7:7c6a9bb2d30e 232 emg_value_l = emg_l.read();
aluminium 7:7c6a9bb2d30e 233 emgFiltered_l = bqc.step( emg_value_l );
aluminium 7:7c6a9bb2d30e 234 filteredAbs_l = abs(emgFiltered_l);
aluminium 7:7c6a9bb2d30e 235
aluminium 7:7c6a9bb2d30e 236
aluminium 7:7c6a9bb2d30e 237 // signal_verzameling[n]= filteredAbs_r;
aluminium 7:7c6a9bb2d30e 238 signal_verzameling_l = signal_verzameling_l + filteredAbs_l ;
aluminium 7:7c6a9bb2d30e 239 wait(0.0004);
aluminium 7:7c6a9bb2d30e 240
aluminium 7:7c6a9bb2d30e 241 if (n == 4999){
aluminium 7:7c6a9bb2d30e 242 avg_emg_l = signal_verzameling_l / n;
aluminium 7:7c6a9bb2d30e 243
aluminium 7:7c6a9bb2d30e 244 }
aluminium 7:7c6a9bb2d30e 245 }
aluminium 7:7c6a9bb2d30e 246
aluminium 7:7c6a9bb2d30e 247 led3.write(1);
aluminium 7:7c6a9bb2d30e 248 // double lengte_array = sizeof(signal_verzameling);
aluminium 7:7c6a9bb2d30e 249 // pc.printf("lengte_array = %f\n\r",lengte_array);
aluminium 7:7c6a9bb2d30e 250
aluminium 7:7c6a9bb2d30e 251 // for(int i = 0; i < lengte_array; i++){
aluminium 7:7c6a9bb2d30e 252
aluminium 7:7c6a9bb2d30e 253
aluminium 7:7c6a9bb2d30e 254 // sum_array = sum_array + signal_verzameling[i] ;
aluminium 7:7c6a9bb2d30e 255 // }
aluminium 7:7c6a9bb2d30e 256 //avg_emg_r = sum_array / lengte_array;
aluminium 7:7c6a9bb2d30e 257 // pc.printf("avg_emg_r = %f\n\r",avg_emg_r);
aluminium 7:7c6a9bb2d30e 258
aluminium 7:7c6a9bb2d30e 259
aluminium 7:7c6a9bb2d30e 260
aluminium 7:7c6a9bb2d30e 261
aluminium 7:7c6a9bb2d30e 262 check_calibration_l=1;
DAkoetsier 2:8f6fca2179f2 263 led3.write(1);
aluminium 5:7c8176cdaa1c 264 // }
aluminium 5:7c8176cdaa1c 265 return 0;
DAkoetsier 2:8f6fca2179f2 266 }
DAkoetsier 2:8f6fca2179f2 267
DAkoetsier 2:8f6fca2179f2 268 // MAIN
DAkoetsier 2:8f6fca2179f2 269
DAkoetsier 2:8f6fca2179f2 270 int main(){
aluminium 5:7c8176cdaa1c 271 pc.baud(115200);
DAkoetsier 2:8f6fca2179f2 272
aluminium 5:7c8176cdaa1c 273 pc.printf("Lampjes zijn langs geweest");
DAkoetsier 2:8f6fca2179f2 274 led1.write(1);
DAkoetsier 2:8f6fca2179f2 275 led2.write(1);
DAkoetsier 2:8f6fca2179f2 276 led3.write(1);
DAkoetsier 2:8f6fca2179f2 277
aluminium 4:285fb7d84088 278
aluminium 4:285fb7d84088 279
DAkoetsier 2:8f6fca2179f2 280 bqc.add( &bq1_low ).add( &bq2_high ).add( &bq3_notch );
DAkoetsier 2:8f6fca2179f2 281
aluminium 7:7c6a9bb2d30e 282
aluminium 7:7c6a9bb2d30e 283 LED_timer_r.attach(&check_emg_r, 0.1); //continously execute the motor controller
aluminium 7:7c6a9bb2d30e 284 LED_timer_l.attach(&check_emg_l, 0.1); //continously execute the motor controller
aluminium 7:7c6a9bb2d30e 285
DAkoetsier 2:8f6fca2179f2 286
aluminium 7:7c6a9bb2d30e 287 calibration_r();
aluminium 7:7c6a9bb2d30e 288 calibration_l();
DAkoetsier 2:8f6fca2179f2 289
DAkoetsier 2:8f6fca2179f2 290 while(1){ //while loop to keep system going
DAkoetsier 2:8f6fca2179f2 291
DAkoetsier 2:8f6fca2179f2 292 }
DAkoetsier 2:8f6fca2179f2 293 }