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@49:818a0e90ed9c, 2016-11-03 (annotated)

Committer:
daniQQue
Date:
Thu Nov 03 09:23:48 2016 +0000
Revision:
49:818a0e90ed9c
Parent:
47:ddaa59d48aca 
met tekst erbij, niks veranderd aan werkende versie;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daniQQue 46:4a8889f9dc9f 1 //=====================================================================================
daniQQue 0:34c739fcc3e0 2 //libraries
daniQQue 49:818a0e90ed9c 3 #include "mbed.h" //mbed revision 113
daniQQue 49:818a0e90ed9c 4 #include "HIDScope.h" //Hidscope by Tom Lankhorst
daniQQue 49:818a0e90ed9c 5 #include "BiQuad.h" //BiQuad by Tom Lankhorst
daniQQue 49:818a0e90ed9c 6 #include "MODSERIAL.h" //Modserial
daniQQue 0:34c739fcc3e0 7
daniQQue 46:4a8889f9dc9f 8
daniQQue 46:4a8889f9dc9f 9 //=====================================================================================
daniQQue 0:34c739fcc3e0 10 //Define objects
daniQQue 49:818a0e90ed9c 11
daniQQue 49:818a0e90ed9c 12 //EMG
daniQQue 40:187ef29de53d 13 AnalogIn emg_biceps_right_in( A0 ); //analog in to get EMG biceps (r) in to c++
daniQQue 40:187ef29de53d 14 AnalogIn emg_triceps_right_in(A1); //analog in to get EMG triceps (r) in to c++
daniQQue 40:187ef29de53d 15 AnalogIn emg_biceps_left_in (A2); //analog in to get EMG biceps (l) in to c++
daniQQue 49:818a0e90ed9c 16
daniQQue 49:818a0e90ed9c 17 //Tickers
daniQQue 49:818a0e90ed9c 18 Ticker sample_timer; //ticker for EMG signal sampling, analog becomes digital
daniQQue 49:818a0e90ed9c 19 Ticker ticker_switch; //ticker for switch, every second it is possible to switch
daniQQue 49:818a0e90ed9c 20
daniQQue 49:818a0e90ed9c 21 //Monitoring
daniQQue 46:4a8889f9dc9f 22 HIDScope scope(5); //open 5 channels in hidscope
daniQQue 46:4a8889f9dc9f 23 MODSERIAL pc(USBTX, USBRX); //pc connection
daniQQue 49:818a0e90ed9c 24 DigitalOut red(LED_RED); //LED on K64F board, 1 is out; 0 is on
daniQQue 49:818a0e90ed9c 25 DigitalOut green(LED_GREEN); //LED on K64f board, 1 is out; o is on
daniQQue 40:187ef29de53d 26
daniQQue 40:187ef29de53d 27 //motors
daniQQue 49:818a0e90ed9c 28 DigitalOut richting_motor1(D7); //motor 1 connected to motor 1 at k64f board; for turningtable
daniQQue 47:ddaa59d48aca 29 PwmOut pwm_motor1(D6);
daniQQue 49:818a0e90ed9c 30 DigitalOut richting_motor2(D4); ///motor 2 connected to motor 2 at k64f board; for linear actuator
daniQQue 47:ddaa59d48aca 31 PwmOut pwm_motor2(D5);
daniQQue 0:34c739fcc3e0 32
daniQQue 46:4a8889f9dc9f 33 //=====================================================================================
daniQQue 0:34c739fcc3e0 34 //define variables
daniQQue 47:ddaa59d48aca 35
daniQQue 49:818a0e90ed9c 36 //tresholds
daniQQue 49:818a0e90ed9c 37 double treshold_biceps_right =0.04; //common values that work.
daniQQue 49:818a0e90ed9c 38 double treshold_biceps_left=-0.04; // tested on multiple persons
daniQQue 49:818a0e90ed9c 39 double treshold_triceps=-0.04; //triceps and left biceps is specified negative, thus negative treshold
daniQQue 49:818a0e90ed9c 40
daniQQue 49:818a0e90ed9c 41 //on/off and switch signals
daniQQue 49:818a0e90ed9c 42 int switch_signal = 0; //start of counter, switch made by even and odd numbers
daniQQue 49:818a0e90ed9c 43 int onoffsignal_biceps;
daniQQue 45:08bddea67bd8 44 int switch_signal_triceps;
daniQQue 46:4a8889f9dc9f 45
daniQQue 49:818a0e90ed9c 46 //motorvariables
daniQQue 49:818a0e90ed9c 47 float speedmotor1=0.18; //speed of motor 1 is 0.18pwm at start
daniQQue 49:818a0e90ed9c 48 float speedmotor2=1.0; //speed of motor 2 is 1.0 pwm at start
daniQQue 46:4a8889f9dc9f 49
daniQQue 49:818a0e90ed9c 50 int cw=0; //clockwise direction
daniQQue 49:818a0e90ed9c 51 int ccw=1; //counterclockwise direction
daniQQue 40:187ef29de53d 52
daniQQue 44:c79e5944ac91 53 //=======================================
daniQQue 44:c79e5944ac91 54 //filter coefficients
daniQQue 40:187ef29de53d 55
daniQQue 40:187ef29de53d 56 //b1 = biceps right arm
daniQQue 49:818a0e90ed9c 57 BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of?
daniQQue 49:818a0e90ed9c 58 BiQuad filternotch1_b1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of?
daniQQue 40:187ef29de53d 59
daniQQue 40:187ef29de53d 60 //t1= triceps right arm
daniQQue 49:818a0e90ed9c 61 BiQuad filterhigh_t1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of?
daniQQue 49:818a0e90ed9c 62 BiQuad filternotch1_t1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of?
daniQQue 5:688b1b5530d8 63
daniQQue 40:187ef29de53d 64 //b2= biceps left arm
daniQQue 49:818a0e90ed9c 65 BiQuad filterhigh_b2(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of?
daniQQue 49:818a0e90ed9c 66 BiQuad filternotch1_b2 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of?
daniQQue 40:187ef29de53d 67
daniQQue 40:187ef29de53d 68 //after abs filtering
daniQQue 49:818a0e90ed9c 69 BiQuad filterlow_b1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of?
daniQQue 49:818a0e90ed9c 70 BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of?
daniQQue 49:818a0e90ed9c 71 BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of?
daniQQue 10:7255b59224cc 72
daniQQue 45:08bddea67bd8 73 //======================================================================
daniQQue 45:08bddea67bd8 74 //voids
daniQQue 45:08bddea67bd8 75 //======================================================================
daniQQue 45:08bddea67bd8 76
daniQQue 40:187ef29de53d 77 //function teller
daniQQue 49:818a0e90ed9c 78 void switch_function() { // The switch function. Makes it possible to switch between the motors. It simply adds one at switch_signal.
daniQQue 45:08bddea67bd8 79 if(switch_signal_triceps==1)
daniQQue 40:187ef29de53d 80 {
daniQQue 45:08bddea67bd8 81 switch_signal++;
daniQQue 49:818a0e90ed9c 82
daniQQue 49:818a0e90ed9c 83 // To monitor what is happening: we will show the text in putty and change led color from red to green or vice versa.
daniQQue 49:818a0e90ed9c 84
daniQQue 40:187ef29de53d 85 green=!green;
daniQQue 40:187ef29de53d 86 red=!red;
daniQQue 47:ddaa59d48aca 87
daniQQue 45:08bddea67bd8 88 if (switch_signal%2==0)
daniQQue 49:818a0e90ed9c 89 {pc.printf("If you contract the biceps, the robot will go right \r\n");
daniQQue 47:ddaa59d48aca 90 pc.printf("If you contract the triceps, the robot will go left \r\n");
daniQQue 47:ddaa59d48aca 91 pc.printf("\r\n");
daniQQue 47:ddaa59d48aca 92 }
daniQQue 47:ddaa59d48aca 93
daniQQue 47:ddaa59d48aca 94
daniQQue 47:ddaa59d48aca 95 else
daniQQue 40:187ef29de53d 96 {pc.printf("If you contract the biceps, the robot will go up \r\n");
daniQQue 40:187ef29de53d 97 pc.printf("If you contract the triceps, the robot will go down \r\n");
daniQQue 47:ddaa59d48aca 98 pc.printf("\r\n");
daniQQue 40:187ef29de53d 99 }
daniQQue 47:ddaa59d48aca 100
daniQQue 40:187ef29de53d 101 }
daniQQue 40:187ef29de53d 102 }
daniQQue 49:818a0e90ed9c 103
daniQQue 49:818a0e90ed9c 104 //======================================================================
daniQQue 49:818a0e90ed9c 105 //functions which are called in ticker to sample the analog signal and make the on/off and switch signal.
daniQQue 40:187ef29de53d 106
daniQQue 0:34c739fcc3e0 107 void filter(){
daniQQue 40:187ef29de53d 108 //biceps right arm read+filtering
daniQQue 49:818a0e90ed9c 109 double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 49:818a0e90ed9c 110 double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); //high pass filter, to remove offset
daniQQue 49:818a0e90ed9c 111 double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); //notch filter, to remove noise
daniQQue 49:818a0e90ed9c 112 double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //rectify the signal, fabs because float
daniQQue 49:818a0e90ed9c 113 double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //low pass filter to envelope the signal
daniQQue 40:187ef29de53d 114
daniQQue 40:187ef29de53d 115 //triceps right arm read+filtering
daniQQue 49:818a0e90ed9c 116 double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 49:818a0e90ed9c 117 double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); //high pass filter, to remove offset
daniQQue 49:818a0e90ed9c 118 double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); //notch filter, to remove noise
daniQQue 49:818a0e90ed9c 119 double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //rectify the signal, fabs because float
daniQQue 49:818a0e90ed9c 120 double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //low pass filter to envelope the signal
daniQQue 7:42d0e38196f1 121
daniQQue 40:187ef29de53d 122 //biceps left arm read+filtering
daniQQue 49:818a0e90ed9c 123 double emg_biceps_left=emg_biceps_left_in.read(); //read the emg value from the elektrodes
daniQQue 49:818a0e90ed9c 124 double emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left); //high pass filter, to remove offset
daniQQue 49:818a0e90ed9c 125 double emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left); //notch filter, to remove noise
daniQQue 49:818a0e90ed9c 126 double emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //rectify the signal, fabs because float
daniQQue 49:818a0e90ed9c 127 double emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left); //low pass filter to envelope the signal
daniQQue 40:187ef29de53d 128
daniQQue 40:187ef29de53d 129 //creating of on/off signal with the created on/off signals, with if statement for right arm!
daniQQue 44:c79e5944ac91 130 //signal substraction of filter biceps and triceps. right Biceps + left biceps -
daniQQue 44:c79e5944ac91 131 double signal_biceps_sum=emg_filtered_biceps_right-emg_filtered_biceps_left;
daniQQue 44:c79e5944ac91 132 double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right;
daniQQue 45:08bddea67bd8 133
daniQQue 44:c79e5944ac91 134 //creating of on/off signal with the created on/off signals, with if statement for right arm!
daniQQue 49:818a0e90ed9c 135 if (signal_biceps_sum>treshold_biceps_right)
daniQQue 49:818a0e90ed9c 136 {onoffsignal_biceps=1;}
daniQQue 7:42d0e38196f1 137
daniQQue 49:818a0e90ed9c 138 else if (signal_biceps_sum<treshold_biceps_left)
daniQQue 49:818a0e90ed9c 139 { onoffsignal_biceps=-1; }
daniQQue 40:187ef29de53d 140
daniQQue 49:818a0e90ed9c 141 else
daniQQue 49:818a0e90ed9c 142 {onoffsignal_biceps=0;}
daniQQue 7:42d0e38196f1 143
daniQQue 40:187ef29de53d 144 //creating on/off signal for switch (left arm)
daniQQue 40:187ef29de53d 145
daniQQue 49:818a0e90ed9c 146 if (bicepstriceps_rightarm<treshold_triceps)
daniQQue 49:818a0e90ed9c 147 { switch_signal_triceps=1; }
daniQQue 40:187ef29de53d 148
daniQQue 40:187ef29de53d 149 else
daniQQue 49:818a0e90ed9c 150 { switch_signal_triceps=0; }
daniQQue 49:818a0e90ed9c 151
daniQQue 49:818a0e90ed9c 152 //send signals to scope to monitor the EMG signals
daniQQue 49:818a0e90ed9c 153 scope.set(0, emg_filtered_biceps_right); //set emg signal of right biceps to scope in channel 0
daniQQue 49:818a0e90ed9c 154 scope.set(1, emg_filtered_triceps_right); // set emg signal of right triceps to scope in channel 1
daniQQue 49:818a0e90ed9c 155 scope.set(2, emg_filtered_biceps_left); // set emg signal of left biceps to scope in channel 2
daniQQue 49:818a0e90ed9c 156 scope.set(3, onoffsignal_biceps); // set on/off signal for the motors to scope in channel 3
daniQQue 49:818a0e90ed9c 157 scope.set(4, switch_signal_triceps); // set the switch signal to scope in channel 4
daniQQue 40:187ef29de53d 158
daniQQue 40:187ef29de53d 159 scope.send(); //send all the signals to the scope
daniQQue 0:34c739fcc3e0 160 }
daniQQue 49:818a0e90ed9c 161 //======================================================================
daniQQue 0:34c739fcc3e0 162 //program
daniQQue 49:818a0e90ed9c 163 //======================================================================
daniQQue 0:34c739fcc3e0 164 int main()
daniQQue 0:34c739fcc3e0 165 {
daniQQue 40:187ef29de53d 166
daniQQue 49:818a0e90ed9c 167 pc.baud(115200); //connect with pc with baudrate 115200
daniQQue 49:818a0e90ed9c 168 green=1; //led is off (1), at beginning
daniQQue 49:818a0e90ed9c 169 red=0; //led is on (0), at beginning
daniQQue 49:818a0e90ed9c 170
daniQQue 49:818a0e90ed9c 171 //attach tickers to functions
daniQQue 0:34c739fcc3e0 172 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 173 ticker_switch.attach(&switch_function,1.0);
daniQQue 44:c79e5944ac91 174
daniQQue 49:818a0e90ed9c 175 //Show the user what the starting motor will be and what will happen
daniQQue 47:ddaa59d48aca 176 pc.printf("We will start the demonstration\r\n");
daniQQue 49:818a0e90ed9c 177 pc.printf("\r\n\r\n\r\n");
daniQQue 49:818a0e90ed9c 178
daniQQue 47:ddaa59d48aca 179
daniQQue 47:ddaa59d48aca 180 if (switch_signal%2==0)
daniQQue 47:ddaa59d48aca 181 {pc.printf("If you contract the biceps, the robot will go right \r\n");
daniQQue 47:ddaa59d48aca 182 pc.printf("If you contract the triceps, the robot will go left \r\n");
daniQQue 47:ddaa59d48aca 183 pc.printf("\r\n");
daniQQue 47:ddaa59d48aca 184 }
daniQQue 47:ddaa59d48aca 185
daniQQue 47:ddaa59d48aca 186
daniQQue 47:ddaa59d48aca 187 else
daniQQue 45:08bddea67bd8 188 {pc.printf("If you contract the biceps, the robot will go up \r\n");
daniQQue 45:08bddea67bd8 189 pc.printf("If you contract the triceps, the robot will go down \r\n");
daniQQue 47:ddaa59d48aca 190 pc.printf("\r\n");
daniQQue 45:08bddea67bd8 191 }
daniQQue 45:08bddea67bd8 192
daniQQue 44:c79e5944ac91 193 //==============================================================================================
daniQQue 0:34c739fcc3e0 194 //endless loop
daniQQue 0:34c739fcc3e0 195
daniQQue 40:187ef29de53d 196
daniQQue 49:818a0e90ed9c 197 while (true) { // neverending loop
daniQQue 8:cd0cb71b69f2 198
daniQQue 45:08bddea67bd8 199 if (onoffsignal_biceps==-1) //left biceps contracted
daniQQue 40:187ef29de53d 200 {
daniQQue 49:818a0e90ed9c 201 if (switch_signal%2==0) //switch even
daniQQue 40:187ef29de53d 202 {
daniQQue 49:818a0e90ed9c 203 richting_motor1 = ccw; //motor 1, left
daniQQue 49:818a0e90ed9c 204 pwm_motor1 = speedmotor1; //speed of motor 1
daniQQue 40:187ef29de53d 205
daniQQue 40:187ef29de53d 206 }
daniQQue 40:187ef29de53d 207
daniQQue 49:818a0e90ed9c 208 else //switch odd
daniQQue 40:187ef29de53d 209 {
daniQQue 49:818a0e90ed9c 210 richting_motor2 = ccw; //motor 2, up
daniQQue 49:818a0e90ed9c 211 pwm_motor2 = speedmotor2;//speed of motor 2
daniQQue 40:187ef29de53d 212
daniQQue 40:187ef29de53d 213 }
daniQQue 40:187ef29de53d 214
daniQQue 8:cd0cb71b69f2 215 }
daniQQue 49:818a0e90ed9c 216 else if (onoffsignal_biceps==1) //right biceps contracted
daniQQue 40:187ef29de53d 217 {
daniQQue 49:818a0e90ed9c 218 if (switch_signal%2==0) //switch signal even
daniQQue 40:187ef29de53d 219 {
daniQQue 49:818a0e90ed9c 220 richting_motor1 = cw; //motor 1, right
daniQQue 49:818a0e90ed9c 221 pwm_motor1 = speedmotor1; //speed motor 1
daniQQue 40:187ef29de53d 222
daniQQue 40:187ef29de53d 223 }
daniQQue 49:818a0e90ed9c 224 else //switch signal odd
daniQQue 40:187ef29de53d 225 {
daniQQue 49:818a0e90ed9c 226 richting_motor2 = cw; //motor 2. down
daniQQue 49:818a0e90ed9c 227 pwm_motor2 = speedmotor2; //speed motor 2
daniQQue 40:187ef29de53d 228
daniQQue 40:187ef29de53d 229 }
daniQQue 40:187ef29de53d 230 }
daniQQue 49:818a0e90ed9c 231 else{
daniQQue 49:818a0e90ed9c 232 //no contraction of biceps
daniQQue 40:187ef29de53d 233 pwm_motor2=0;
daniQQue 40:187ef29de53d 234 pwm_motor1=0;
daniQQue 40:187ef29de53d 235 }
daniQQue 40:187ef29de53d 236
daniQQue 49:818a0e90ed9c 237 }//while true closed
daniQQue 0:34c739fcc3e0 238
daniQQue 49:818a0e90ed9c 239 } //int main closed
daniQQue 49:818a0e90ed9c 240
daniQQue 49:818a0e90ed9c 241 //=============================================================================================1