Daniqe Kottelenberg / Mbed 2 deprecated another_try_from_scratch_on_emg

emg with text

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Fork of emg_import by Daniqe Kottelenberg

main.cpp@47:ddaa59d48aca, 2016-11-03 (annotated)

Committer:
daniQQue
Date:
Thu Nov 03 08:43:08 2016 +0000
Revision:
47:ddaa59d48aca
Parent:
46:4a8889f9dc9f
Child:
48:0a16643c9de4
Child:
49:818a0e90ed9c
motor 1, op motor 1 aangesloten. rood: links rechts, groen, omhoog naar beneden

Who changed what in which revision?

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