Projectgroep 20 Biorobotics / Mbed 2 deprecated scriptjevanjongen

Deze werkt van de jongen

Dependencies:   Encoder HIDScope MODSERIAL biquadFilter mbed

main.cpp@0:ae9240e8af8c, 2017-10-30 (annotated)

Committer:
paulineoonk
Date:
Mon Oct 30 16:11:48 2017 +0000
Revision:
0:ae9240e8af8c
..

Who changed what in which revision?

UserRevisionLine numberNew contents of line
paulineoonk 0:ae9240e8af8c 1 #include "mbed.h"
paulineoonk 0:ae9240e8af8c 2 #include "HIDScope.h"
paulineoonk 0:ae9240e8af8c 3 #include "BiQuad.h" //require the HIDScope library
paulineoonk 0:ae9240e8af8c 4 #include "MODSERIAL.h"
paulineoonk 0:ae9240e8af8c 5 //Define objects
paulineoonk 0:ae9240e8af8c 6 AnalogIn emg(A0); //analog of EMG input
paulineoonk 0:ae9240e8af8c 7 Ticker sample_timer;
paulineoonk 0:ae9240e8af8c 8 Ticker motor_timer;
paulineoonk 0:ae9240e8af8c 9 Ticker cal_timer;
paulineoonk 0:ae9240e8af8c 10 HIDScope scope(2); //instantize a 2-channel HIDScope object
paulineoonk 0:ae9240e8af8c 11 DigitalIn button1(PTA4); //test button for starting motor 1
paulineoonk 0:ae9240e8af8c 12 DigitalOut led1(LED_RED);
paulineoonk 0:ae9240e8af8c 13 DigitalOut led2(LED_BLUE);
paulineoonk 0:ae9240e8af8c 14 MODSERIAL pc(USBTX,USBRX);
paulineoonk 0:ae9240e8af8c 15
paulineoonk 0:ae9240e8af8c 16 //The biquad filters required to transform the EMG signal into an usable signal
paulineoonk 0:ae9240e8af8c 17 BiQuad filterhigh1(-1.1430, 0.4128, 0.6389, -1.2779, 0.6389);
paulineoonk 0:ae9240e8af8c 18 BiQuad filterlow1(1.9556, 0.9565, 0.9780, 1.9561, 0.9780);
paulineoonk 0:ae9240e8af8c 19 BiQuad notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780);
paulineoonk 0:ae9240e8af8c 20 BiQuad filterlow2(-1.9645, 0.9651, 1.5515e-4, 3.1030e-4, 1.5515e-4);
paulineoonk 0:ae9240e8af8c 21 double emg_value;
paulineoonk 0:ae9240e8af8c 22 double signalpart1;
paulineoonk 0:ae9240e8af8c 23 double signalpart2;
paulineoonk 0:ae9240e8af8c 24 double signalpart3;
paulineoonk 0:ae9240e8af8c 25 double signalpart4;
paulineoonk 0:ae9240e8af8c 26 double signalfinal;
paulineoonk 0:ae9240e8af8c 27 double onoffsignal;
paulineoonk 0:ae9240e8af8c 28 double maxcal=0;
paulineoonk 0:ae9240e8af8c 29 bool calyes=0;
paulineoonk 0:ae9240e8af8c 30 Timer looptime;
paulineoonk 0:ae9240e8af8c 31
paulineoonk 0:ae9240e8af8c 32
paulineoonk 0:ae9240e8af8c 33 void filter(){ //filter function
paulineoonk 0:ae9240e8af8c 34 looptime.start();
paulineoonk 0:ae9240e8af8c 35 if(calyes==1){
paulineoonk 0:ae9240e8af8c 36 emg_value = emg.read(); //read the emg value from the elektrodes
paulineoonk 0:ae9240e8af8c 37 signalpart1 = notch.step(emg_value); //Highpass filter for removing offset and artifacts
paulineoonk 0:ae9240e8af8c 38 signalpart2 = filterhigh1.step(signalpart1);//rectify the filtered signal
paulineoonk 0:ae9240e8af8c 39 signalpart3 = abs(signalpart2); //low pass filter to envelope the emg
paulineoonk 0:ae9240e8af8c 40 signalpart4 = filterlow1.step(signalpart3); //notch filter to remove 50Hz signal
paulineoonk 0:ae9240e8af8c 41 signalfinal = filterlow2.step(signalpart4); //2nd low pass filter to envelope the emg
paulineoonk 0:ae9240e8af8c 42 onoffsignal=signalfinal/maxcal; //divide the emg signal by the max EMG to calibrate the signal per person
paulineoonk 0:ae9240e8af8c 43 scope.set(0,emg_value); //set emg signal to scope in channel 1
paulineoonk 0:ae9240e8af8c 44 scope.set(1,onoffsignal); //set filtered signal to scope in channel 2
paulineoonk 0:ae9240e8af8c 45 scope.send(); //send the signals to the scope
paulineoonk 0:ae9240e8af8c 46 pc.printf("emg %f, filtered %f, loop %f \r\n",emg_value,onoffsignal, looptime.read());
paulineoonk 0:ae9240e8af8c 47 looptime.reset();
paulineoonk 0:ae9240e8af8c 48 }
paulineoonk 0:ae9240e8af8c 49 }
paulineoonk 0:ae9240e8af8c 50 void checkmotor(){ //check the normalized signal and set actions if a threshold is passed
paulineoonk 0:ae9240e8af8c 51 if(calyes==1){ //if signal passes threshold value, red light goes on
paulineoonk 0:ae9240e8af8c 52 if(onoffsignal<=0.25){
paulineoonk 0:ae9240e8af8c 53 led1.write(1);
paulineoonk 0:ae9240e8af8c 54 led2.write(0);
paulineoonk 0:ae9240e8af8c 55 }
paulineoonk 0:ae9240e8af8c 56 else if(onoffsignal >= 0.5){ //if signal does not pass threshold value, blue light goes on
paulineoonk 0:ae9240e8af8c 57 led1.write(0);
paulineoonk 0:ae9240e8af8c 58 led2.write(1);
paulineoonk 0:ae9240e8af8c 59 }
paulineoonk 0:ae9240e8af8c 60 }
paulineoonk 0:ae9240e8af8c 61 }
paulineoonk 0:ae9240e8af8c 62
paulineoonk 0:ae9240e8af8c 63 void calibri(){ //calibration function
paulineoonk 0:ae9240e8af8c 64 if(button1.read()==false){
paulineoonk 0:ae9240e8af8c 65 for(int n =0; n<5000;n++){ //read for 5000 samples as calibration
paulineoonk 0:ae9240e8af8c 66 emg_value = emg.read(); //read the emg value from the elektrodes
paulineoonk 0:ae9240e8af8c 67 signalpart1 = notch.step(emg_value); //Highpass filter for removing offset and artifacts
paulineoonk 0:ae9240e8af8c 68 signalpart2 = filterhigh1.step(signalpart1); //rectify the filtered signal
paulineoonk 0:ae9240e8af8c 69 signalpart3 = abs(signalpart2); //low pass filter to envelope the emg
paulineoonk 0:ae9240e8af8c 70 signalpart4 = filterlow1.step(signalpart3); //notch filter to remove 50Hz signal
paulineoonk 0:ae9240e8af8c 71 signalfinal = filterlow2.step(signalpart4); //2nd low pass filter to envelope the emg
paulineoonk 0:ae9240e8af8c 72 double signalmeasure = signalfinal;
paulineoonk 0:ae9240e8af8c 73 if (signalmeasure > maxcal){ //determine what the highest reachable emg signal is
paulineoonk 0:ae9240e8af8c 74 maxcal = signalmeasure;
paulineoonk 0:ae9240e8af8c 75 }
paulineoonk 0:ae9240e8af8c 76 }
paulineoonk 0:ae9240e8af8c 77 calyes=1;
paulineoonk 0:ae9240e8af8c 78 }
paulineoonk 0:ae9240e8af8c 79 }
paulineoonk 0:ae9240e8af8c 80 int main() //main loop
paulineoonk 0:ae9240e8af8c 81 {
paulineoonk 0:ae9240e8af8c 82 pc.baud(115200); //value required for putty
paulineoonk 0:ae9240e8af8c 83 led1.write(1);
paulineoonk 0:ae9240e8af8c 84 led2.write(1);
paulineoonk 0:ae9240e8af8c 85
paulineoonk 0:ae9240e8af8c 86 sample_timer.attach(&filter, 0.002); //continously execute the EMG reader and filter
paulineoonk 0:ae9240e8af8c 87 motor_timer.attach(&checkmotor, 0.002); //continously execute the motor controller
paulineoonk 0:ae9240e8af8c 88 cal_timer.attach(&calibri, 0.002); //ticker to check if motor is being calibrated
paulineoonk 0:ae9240e8af8c 89 pc.printf("%d",signalfinal);
paulineoonk 0:ae9240e8af8c 90
paulineoonk 0:ae9240e8af8c 91 while(1){
paulineoonk 0:ae9240e8af8c 92 //while loop to keep system going
paulineoonk 0:ae9240e8af8c 93 }
paulineoonk 0:ae9240e8af8c 94 }