emg with text

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Fork of emg_import by Daniqe Kottelenberg

Committer:
daniQQue
Date:
Tue Nov 01 08:48:08 2016 +0000
Revision:
40:187ef29de53d
Parent:
39:c933a6c2b730
Child:
41:9ea3d5921f07
Child:
44:c79e5944ac91
werkend, met tekst in putty en lampje aan/uit. Zonder kalibratie

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daniQQue 0:34c739fcc3e0 1 //libraries
daniQQue 0:34c739fcc3e0 2 #include "mbed.h"
daniQQue 0:34c739fcc3e0 3 #include "HIDScope.h"
daniQQue 14:5b17697cf775 4 #include "BiQuad.h"
daniQQue 40:187ef29de53d 5 #include "MODSERIAL.h"
daniQQue 0:34c739fcc3e0 6
daniQQue 0:34c739fcc3e0 7 //Define objects
daniQQue 40:187ef29de53d 8 AnalogIn emg_biceps_right_in( A0 ); //analog in to get EMG biceps (r) in to c++
daniQQue 40:187ef29de53d 9 AnalogIn emg_triceps_right_in(A1); //analog in to get EMG triceps (r) in to c++
daniQQue 40:187ef29de53d 10 AnalogIn emg_biceps_left_in (A2); //analog in to get EMG biceps (l) in to c++
daniQQue 0:34c739fcc3e0 11 Ticker sample_timer; //ticker
daniQQue 40:187ef29de53d 12 Ticker switch_function; //ticker
daniQQue 40:187ef29de53d 13 HIDScope scope(5); //open 3 channels in hidscope
daniQQue 40:187ef29de53d 14 MODSERIAL pc(USBTX, USBRX); //pc connection
daniQQue 40:187ef29de53d 15 DigitalOut red(LED_RED);
daniQQue 40:187ef29de53d 16 DigitalOut green(LED_GREEN);
daniQQue 40:187ef29de53d 17
daniQQue 40:187ef29de53d 18 //motors
daniQQue 40:187ef29de53d 19 DigitalOut richting_motor1(D4);
daniQQue 40:187ef29de53d 20 PwmOut pwm_motor1(D5);
daniQQue 40:187ef29de53d 21 DigitalOut richting_motor2(D7);
daniQQue 40:187ef29de53d 22 PwmOut pwm_motor2(D6);
daniQQue 0:34c739fcc3e0 23
daniQQue 0:34c739fcc3e0 24 //define variables
daniQQue 40:187ef29de53d 25 //other
daniQQue 40:187ef29de53d 26 int onoffsignal_rightarm=0; // on/off signal: 1; biceps activation, 0: nothing, -1, triceps activation
daniQQue 40:187ef29de53d 27 int switch_signal_leftarm=0; // switching between motors.
daniQQue 40:187ef29de53d 28 double cut_off_value_biceps =0.04; //gespecificeerd door floortje
daniQQue 40:187ef29de53d 29 double cut_off_value_triceps=-0.03; //gespecificeerd door floortje
daniQQue 40:187ef29de53d 30 double signal_right_arm;
daniQQue 40:187ef29de53d 31 int n = 0; //start van de teller wordt op nul gesteld
daniQQue 40:187ef29de53d 32
daniQQue 40:187ef29de53d 33 //biceps arm 1, right arm
daniQQue 15:bb4a6c7836d8 34 double emg_biceps_right;
daniQQue 15:bb4a6c7836d8 35 double emg_filtered_high_biceps_right;
daniQQue 15:bb4a6c7836d8 36 double emg_abs_biceps_right;
daniQQue 15:bb4a6c7836d8 37 double emg_filtered_biceps_right;
daniQQue 40:187ef29de53d 38 double emg_filtered_high_notch_1_biceps_right;
daniQQue 40:187ef29de53d 39 //double emg_filtered_high_notch_1_2_biceps_right;
daniQQue 40:187ef29de53d 40
daniQQue 40:187ef29de53d 41 //triceps arm 1, right arm
daniQQue 40:187ef29de53d 42 double emg_triceps_right;
daniQQue 40:187ef29de53d 43 double emg_filtered_high_triceps_right;
daniQQue 40:187ef29de53d 44 double emg_abs_triceps_right;
daniQQue 40:187ef29de53d 45 double emg_filtered_triceps_right;
daniQQue 40:187ef29de53d 46 double emg_filtered_high_notch_1_triceps_right;
daniQQue 40:187ef29de53d 47
daniQQue 40:187ef29de53d 48 //biceps arm 1, left arm
daniQQue 40:187ef29de53d 49 double emg_biceps_left;
daniQQue 40:187ef29de53d 50 double emg_filtered_high_biceps_left;
daniQQue 40:187ef29de53d 51 double emg_abs_biceps_left;
daniQQue 40:187ef29de53d 52 double emg_filtered_biceps_left;
daniQQue 40:187ef29de53d 53 double emg_filtered_high_notch_1_biceps_left;
daniQQue 40:187ef29de53d 54
daniQQue 40:187ef29de53d 55 //before abs filtering
daniQQue 40:187ef29de53d 56
daniQQue 40:187ef29de53d 57 //b1 = biceps right arm
daniQQue 40:187ef29de53d 58 BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
daniQQue 40:187ef29de53d 59 BiQuad filternotch1_b1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
daniQQue 40:187ef29de53d 60
daniQQue 40:187ef29de53d 61 //t1= triceps right arm
daniQQue 40:187ef29de53d 62 BiQuad filterhigh_t1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
daniQQue 40:187ef29de53d 63 BiQuad filternotch1_t1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
daniQQue 5:688b1b5530d8 64
daniQQue 40:187ef29de53d 65 //b2= biceps left arm
daniQQue 40:187ef29de53d 66 BiQuad filterhigh_b2(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
daniQQue 40:187ef29de53d 67 BiQuad filternotch1_b2 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
daniQQue 40:187ef29de53d 68
daniQQue 40:187ef29de53d 69 //after abs filtering
daniQQue 40:187ef29de53d 70 BiQuad filterlow_b1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
daniQQue 40:187ef29de53d 71 BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
daniQQue 40:187ef29de53d 72 BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
daniQQue 10:7255b59224cc 73
daniQQue 40:187ef29de53d 74 //function teller
daniQQue 40:187ef29de53d 75 void SwitchN() { // maakt simpele functie die 1 bij n optelt
daniQQue 40:187ef29de53d 76 if(switch_signal_leftarm==1)
daniQQue 40:187ef29de53d 77 {
daniQQue 40:187ef29de53d 78 n++;
daniQQue 40:187ef29de53d 79 green=!green;
daniQQue 40:187ef29de53d 80 red=!red;
daniQQue 40:187ef29de53d 81 if (n%2==0)
daniQQue 40:187ef29de53d 82 {pc.printf("If you contract the biceps, the robot will go up \r\n");
daniQQue 40:187ef29de53d 83 pc.printf("If you contract the triceps, the robot will go down \r\n");
daniQQue 40:187ef29de53d 84 }
daniQQue 40:187ef29de53d 85
daniQQue 40:187ef29de53d 86 else
daniQQue 40:187ef29de53d 87 {pc.printf("If you contract the biceps, the robot will go right \r\n");
daniQQue 40:187ef29de53d 88 pc.printf("If you contract the triceps, the robot will go left \r\n");
daniQQue 40:187ef29de53d 89 }
daniQQue 40:187ef29de53d 90
daniQQue 40:187ef29de53d 91 }
daniQQue 40:187ef29de53d 92 }
daniQQue 40:187ef29de53d 93
daniQQue 40:187ef29de53d 94 //functions which are called in ticker to sample the analog signal
daniQQue 40:187ef29de53d 95
daniQQue 0:34c739fcc3e0 96 void filter(){
daniQQue 40:187ef29de53d 97 //biceps right arm read+filtering
daniQQue 15:bb4a6c7836d8 98 emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 40:187ef29de53d 99 emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right);
daniQQue 40:187ef29de53d 100 emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right);
daniQQue 40:187ef29de53d 101 //emg_filtered_high_notch_1_2_biceps_right=filternotch2_b1.step(emg_filtered_high_notch_1_biceps_right);
daniQQue 40:187ef29de53d 102 emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float
daniQQue 40:187ef29de53d 103 emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right);
daniQQue 40:187ef29de53d 104
daniQQue 40:187ef29de53d 105 //triceps right arm read+filtering
daniQQue 40:187ef29de53d 106 emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 40:187ef29de53d 107 emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right);
daniQQue 40:187ef29de53d 108 emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right);
daniQQue 40:187ef29de53d 109 //emg_filtered_high_notch_1_2_triceps_right=filternotch2_t1.step(emg_filtered_high_notch_1_triceps_right);
daniQQue 40:187ef29de53d 110 emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //fabs because float
daniQQue 40:187ef29de53d 111 emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right);
daniQQue 7:42d0e38196f1 112
daniQQue 40:187ef29de53d 113 //biceps left arm read+filtering
daniQQue 40:187ef29de53d 114 emg_biceps_left=emg_biceps_left_in.read(); //read the emg value from the elektrodes
daniQQue 40:187ef29de53d 115 emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left);
daniQQue 40:187ef29de53d 116 emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left);
daniQQue 40:187ef29de53d 117 emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //fabs because float
daniQQue 40:187ef29de53d 118 emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left);
daniQQue 40:187ef29de53d 119
daniQQue 40:187ef29de53d 120 //signal substraction of filter biceps and triceps. Biceps +,triceps -
daniQQue 40:187ef29de53d 121 signal_right_arm=emg_filtered_biceps_right-emg_filtered_triceps_right;
daniQQue 40:187ef29de53d 122
daniQQue 40:187ef29de53d 123 //creating of on/off signal with the created on/off signals, with if statement for right arm!
daniQQue 40:187ef29de53d 124 if (signal_right_arm>cut_off_value_biceps)
daniQQue 40:187ef29de53d 125 {onoffsignal_rightarm=1;}
daniQQue 7:42d0e38196f1 126
daniQQue 40:187ef29de53d 127 else if (signal_right_arm<cut_off_value_triceps)
daniQQue 40:187ef29de53d 128 {
daniQQue 40:187ef29de53d 129 onoffsignal_rightarm=-1;
daniQQue 40:187ef29de53d 130 }
daniQQue 40:187ef29de53d 131
daniQQue 40:187ef29de53d 132 else
daniQQue 40:187ef29de53d 133 {onoffsignal_rightarm=0;}
daniQQue 7:42d0e38196f1 134
daniQQue 40:187ef29de53d 135 //creating on/off signal for switch (left arm)
daniQQue 40:187ef29de53d 136
daniQQue 40:187ef29de53d 137 if (emg_filtered_biceps_left>cut_off_value_biceps)
daniQQue 40:187ef29de53d 138 {
daniQQue 40:187ef29de53d 139 switch_signal_leftarm=1;
daniQQue 40:187ef29de53d 140 }
daniQQue 40:187ef29de53d 141
daniQQue 40:187ef29de53d 142 else
daniQQue 40:187ef29de53d 143 {
daniQQue 40:187ef29de53d 144 switch_signal_leftarm=0;
daniQQue 40:187ef29de53d 145 }
daniQQue 40:187ef29de53d 146
daniQQue 0:34c739fcc3e0 147 //send signals to scope
daniQQue 40:187ef29de53d 148 scope.set(0, emg_filtered_biceps_right); //set emg signal to scope in channel 0
daniQQue 40:187ef29de53d 149 scope.set(1, emg_filtered_triceps_right); // set emg signal to scope in channel 1
daniQQue 40:187ef29de53d 150 scope.set(2, emg_filtered_biceps_left); // set emg signal to scope in channel 2
daniQQue 40:187ef29de53d 151 scope.set(3, onoffsignal_rightarm); // set emg signal to scope in channel 3
daniQQue 40:187ef29de53d 152 scope.set(4, switch_signal_leftarm);
daniQQue 40:187ef29de53d 153
daniQQue 40:187ef29de53d 154 scope.send(); //send all the signals to the scope
daniQQue 0:34c739fcc3e0 155 }
daniQQue 0:34c739fcc3e0 156
daniQQue 0:34c739fcc3e0 157 //program
daniQQue 0:34c739fcc3e0 158
daniQQue 0:34c739fcc3e0 159 int main()
daniQQue 0:34c739fcc3e0 160 {
daniQQue 40:187ef29de53d 161 pc.baud(115200);
daniQQue 40:187ef29de53d 162 green=0;
daniQQue 40:187ef29de53d 163 red=1;
daniQQue 40:187ef29de53d 164
daniQQue 0:34c739fcc3e0 165 sample_timer.attach(&filter, 0.001); //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds
daniQQue 40:187ef29de53d 166 switch_function.attach(&SwitchN,1.0);
daniQQue 0:34c739fcc3e0 167 //endless loop
daniQQue 0:34c739fcc3e0 168
daniQQue 40:187ef29de53d 169
daniQQue 40:187ef29de53d 170 while (true) { // zorgt er voor dat de code oneindig doorgelopen wordt
daniQQue 40:187ef29de53d 171
daniQQue 8:cd0cb71b69f2 172
daniQQue 40:187ef29de53d 173 if (onoffsignal_rightarm==-1) // als s ingedrukt wordt gebeurd het volgende
daniQQue 40:187ef29de53d 174 {
daniQQue 40:187ef29de53d 175 if (n%2==0) // als s ingedrukt wordt en het getal is even gebeurd het onderstaande
daniQQue 40:187ef29de53d 176 {
daniQQue 40:187ef29de53d 177 richting_motor1 = 1;
daniQQue 40:187ef29de53d 178 pwm_motor1 = 1;
daniQQue 40:187ef29de53d 179
daniQQue 40:187ef29de53d 180 }
daniQQue 40:187ef29de53d 181
daniQQue 40:187ef29de53d 182 else // als s is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande
daniQQue 40:187ef29de53d 183 {
daniQQue 40:187ef29de53d 184 richting_motor2 = 1;
daniQQue 40:187ef29de53d 185 pwm_motor2 = 1;
daniQQue 40:187ef29de53d 186
daniQQue 40:187ef29de53d 187 }
daniQQue 40:187ef29de53d 188
daniQQue 8:cd0cb71b69f2 189 }
daniQQue 40:187ef29de53d 190 else if (onoffsignal_rightarm==1) // als d ingedrukt wordt gebeurd het volgende
daniQQue 40:187ef29de53d 191 {
daniQQue 40:187ef29de53d 192 if (n%2==0) // als d is ingedrukt en n is even dan gebeurd het volgende
daniQQue 40:187ef29de53d 193 {
daniQQue 40:187ef29de53d 194 richting_motor1 = 0;
daniQQue 40:187ef29de53d 195 pwm_motor1 = 1;
daniQQue 40:187ef29de53d 196
daniQQue 40:187ef29de53d 197 }
daniQQue 40:187ef29de53d 198 else // als d is ingedrukt maar het getal is niet even (dus oneven) gebeurt het onderstaande
daniQQue 40:187ef29de53d 199 {
daniQQue 40:187ef29de53d 200 richting_motor2 = 0;
daniQQue 40:187ef29de53d 201 pwm_motor2 = 1;
daniQQue 40:187ef29de53d 202
daniQQue 40:187ef29de53d 203 }
daniQQue 40:187ef29de53d 204 }
daniQQue 40:187ef29de53d 205 else{
daniQQue 40:187ef29de53d 206
daniQQue 40:187ef29de53d 207 pwm_motor2=0;
daniQQue 40:187ef29de53d 208 pwm_motor1=0;
daniQQue 40:187ef29de53d 209 }
daniQQue 40:187ef29de53d 210
daniQQue 40:187ef29de53d 211 }
daniQQue 0:34c739fcc3e0 212
daniQQue 0:34c739fcc3e0 213 }