Rick Koetsier / Mbed 2 deprecated Filter

Fix even de calibratie

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Calibrationscript.cpp@3:e234ad524323, 2017-10-31 (annotated)

Committer:
DAkoetsier
Date:
Tue Oct 31 08:17:07 2017 +0000
Revision:
3:e234ad524323
Parent:
2:8f6fca2179f2 
Niks veranderd;

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"
DAkoetsier 2:8f6fca2179f2 5
DAkoetsier 2:8f6fca2179f2 6 // DEFINING
DAkoetsier 2:8f6fca2179f2 7 //Define Inputs
DAkoetsier 2:8f6fca2179f2 8 AnalogIn emg(A0); //analog of EMG input
DAkoetsier 2:8f6fca2179f2 9 InterruptIn button1(PTA4); //test button for starting motor 1
DAkoetsier 2:8f6fca2179f2 10 InterruptIn button2(SW2); //FOR DEBUGGING
DAkoetsier 2:8f6fca2179f2 11
DAkoetsier 2:8f6fca2179f2 12 //Define Outputs
DAkoetsier 2:8f6fca2179f2 13 DigitalOut led1(LED_RED);
DAkoetsier 2:8f6fca2179f2 14 DigitalOut led2(LED_BLUE);
DAkoetsier 2:8f6fca2179f2 15 DigitalOut led3(LED_GREEN); //FOR DEBUGGING
DAkoetsier 2:8f6fca2179f2 16 MODSERIAL pc(USBTX,USBRX);
DAkoetsier 2:8f6fca2179f2 17
DAkoetsier 2:8f6fca2179f2 18
DAkoetsier 2:8f6fca2179f2 19 //Define Tickers
DAkoetsier 2:8f6fca2179f2 20 Ticker sample_timer; // Taking samples
DAkoetsier 2:8f6fca2179f2 21 Ticker LED_timer; // Write the LED
DAkoetsier 2:8f6fca2179f2 22 Ticker calibration_timer; // Check when to start calibration
DAkoetsier 2:8f6fca2179f2 23 //MODSERIAL pc(USBTX,USBRX);
DAkoetsier 2:8f6fca2179f2 24
DAkoetsier 2:8f6fca2179f2 25 //Define HIDscope
DAkoetsier 2:8f6fca2179f2 26 HIDScope scope(2); //instantize a 2-channel HIDScope object
DAkoetsier 2:8f6fca2179f2 27
DAkoetsier 2:8f6fca2179f2 28
DAkoetsier 2:8f6fca2179f2 29 //Define filters and define the floats which contains the values.
DAkoetsier 2:8f6fca2179f2 30 BiQuadChain bqc;
DAkoetsier 2:8f6fca2179f2 31 BiQuad bq1_low(1.34871e-3, 2.69742e-3, 1.34871e-3, -1.89346, 0.89885);
DAkoetsier 2:8f6fca2179f2 32 BiQuad bq2_high(0.96508, -1.93016, 0.96508, -1.92894, 0.93138);
DAkoetsier 2:8f6fca2179f2 33 BiQuad bq3_notch(0.91859, -1.82269, 0.91859, -1.82269, 0.83718);
DAkoetsier 2:8f6fca2179f2 34
DAkoetsier 2:8f6fca2179f2 35 double emgFiltered;
DAkoetsier 2:8f6fca2179f2 36 double filteredAbs;
DAkoetsier 2:8f6fca2179f2 37 double emg_value;
DAkoetsier 2:8f6fca2179f2 38 double onoffsignal;
DAkoetsier 2:8f6fca2179f2 39 double maxcal=0;
DAkoetsier 2:8f6fca2179f2 40 bool aboveTreshold=0;
DAkoetsier 2:8f6fca2179f2 41
DAkoetsier 2:8f6fca2179f2 42 // FUNCTIONS
DAkoetsier 2:8f6fca2179f2 43 //function for filtering
DAkoetsier 2:8f6fca2179f2 44
DAkoetsier 2:8f6fca2179f2 45 void filter(){
DAkoetsier 2:8f6fca2179f2 46 if(aboveTreshold==1){
DAkoetsier 2:8f6fca2179f2 47
DAkoetsier 2:8f6fca2179f2 48 emg_value = emg.read();
DAkoetsier 2:8f6fca2179f2 49 emgFiltered = bqc.step( emg_value );
DAkoetsier 2:8f6fca2179f2 50 filteredAbs = abs( emgFiltered );
DAkoetsier 2:8f6fca2179f2 51
DAkoetsier 2:8f6fca2179f2 52 onoffsignal=filteredAbs/maxcal; //divide the emg signal by the max EMG to calibrate the signal per person
DAkoetsier 2:8f6fca2179f2 53 scope.set(0,emg_value); //set emg signal to scope in channel 1
DAkoetsier 2:8f6fca2179f2 54 scope.set(1,onoffsignal); //set filtered signal to scope in channel 2
DAkoetsier 2:8f6fca2179f2 55 scope.send(); //send the signals to the scope
DAkoetsier 2:8f6fca2179f2 56 // pc.printf("emg signal %f, filtered signal %f \n",emg_value,onoffsignal);
DAkoetsier 2:8f6fca2179f2 57 }
DAkoetsier 2:8f6fca2179f2 58 }
DAkoetsier 2:8f6fca2179f2 59
DAkoetsier 2:8f6fca2179f2 60 //function to check the threshold
DAkoetsier 2:8f6fca2179f2 61 void checkthreshold(){
DAkoetsier 2:8f6fca2179f2 62 if(aboveTreshold==1){ //if signal passes threshold value, red light goes on
DAkoetsier 2:8f6fca2179f2 63 if(onoffsignal<=0.25){
DAkoetsier 2:8f6fca2179f2 64 led1.write(1);
DAkoetsier 2:8f6fca2179f2 65 led2.write(0);
DAkoetsier 2:8f6fca2179f2 66 }
DAkoetsier 2:8f6fca2179f2 67 else if(onoffsignal >= 0.5){ //if signal does not pass threshold value, blue light goes on
DAkoetsier 2:8f6fca2179f2 68 led1.write(0);
DAkoetsier 2:8f6fca2179f2 69 led2.write(1);
DAkoetsier 2:8f6fca2179f2 70 }
DAkoetsier 2:8f6fca2179f2 71 }
DAkoetsier 2:8f6fca2179f2 72 }
DAkoetsier 2:8f6fca2179f2 73
DAkoetsier 2:8f6fca2179f2 74 //function to calibrate
DAkoetsier 2:8f6fca2179f2 75 void calibration(){
DAkoetsier 2:8f6fca2179f2 76 if(button1.read()==false){
DAkoetsier 2:8f6fca2179f2 77 led3.write(0);
DAkoetsier 2:8f6fca2179f2 78 for(int n =0; n<5000;n++){ //read for 5000 samples as calibration
DAkoetsier 2:8f6fca2179f2 79 led3.write(0);
DAkoetsier 2:8f6fca2179f2 80 emg_value = emg.read();
DAkoetsier 2:8f6fca2179f2 81 emgFiltered = bqc.step( emg_value );
DAkoetsier 2:8f6fca2179f2 82 filteredAbs = abs( emgFiltered );
DAkoetsier 2:8f6fca2179f2 83
DAkoetsier 2:8f6fca2179f2 84 double signalmeasure = filteredAbs;
DAkoetsier 2:8f6fca2179f2 85 if (signalmeasure > maxcal){ //determine what the highest reachable emg signal is
DAkoetsier 2:8f6fca2179f2 86 maxcal = signalmeasure;
DAkoetsier 2:8f6fca2179f2 87 }
DAkoetsier 2:8f6fca2179f2 88 }
DAkoetsier 2:8f6fca2179f2 89 aboveTreshold=1;
DAkoetsier 2:8f6fca2179f2 90 led3.write(1);
DAkoetsier 2:8f6fca2179f2 91 }
DAkoetsier 2:8f6fca2179f2 92 }
DAkoetsier 2:8f6fca2179f2 93
DAkoetsier 2:8f6fca2179f2 94 // MAIN
DAkoetsier 2:8f6fca2179f2 95
DAkoetsier 2:8f6fca2179f2 96 int main(){
DAkoetsier 2:8f6fca2179f2 97 // pc.baud(115200);
DAkoetsier 2:8f6fca2179f2 98
DAkoetsier 2:8f6fca2179f2 99 // pc.printf("Lampjes zijn langs geweest");
DAkoetsier 2:8f6fca2179f2 100 led1.write(1);
DAkoetsier 2:8f6fca2179f2 101 led2.write(1);
DAkoetsier 2:8f6fca2179f2 102 led3.write(1);
DAkoetsier 2:8f6fca2179f2 103
DAkoetsier 2:8f6fca2179f2 104 bqc.add( &bq1_low ).add( &bq2_high ).add( &bq3_notch );
DAkoetsier 2:8f6fca2179f2 105
DAkoetsier 2:8f6fca2179f2 106 sample_timer.attach(&filter, 0.002); //continously execute the EMG reader and filter
DAkoetsier 2:8f6fca2179f2 107 LED_timer.attach(&checkthreshold, 0.002); //continously execute the motor controller
DAkoetsier 2:8f6fca2179f2 108 calibration_timer.attach(&calibration, 0.002); //ticker to check if EMG is being calibrated
DAkoetsier 2:8f6fca2179f2 109 // pc.printf("%d",filteredAbs);
DAkoetsier 2:8f6fca2179f2 110
DAkoetsier 2:8f6fca2179f2 111
DAkoetsier 2:8f6fca2179f2 112
DAkoetsier 2:8f6fca2179f2 113 while(1){ //while loop to keep system going
DAkoetsier 2:8f6fca2179f2 114
DAkoetsier 2:8f6fca2179f2 115 }
DAkoetsier 2:8f6fca2179f2 116 }