K K / Mbed 2 deprecated EMG_movement_code

Combination code of movement and emg code with small changes for 2 motors.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of EMG_converter_code by Gerlinde van de Haar

main.cpp@12:86b3885a6308, 2015-10-28 (annotated)

Committer:
Technical_Muffin
Date:
Wed Oct 28 13:14:16 2015 +0000
Revision:
12:86b3885a6308
Parent:
11:8196434745b4 
Working emg movement code. Apparently biquads need to be doubled and at seperate locations

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Technical_Muffin 0:1883d922ada8 1 #include "mbed.h"
Technical_Muffin 0:1883d922ada8 2 #include "HIDScope.h"
Technical_Muffin 0:1883d922ada8 3 #include "biquadFilter.h" // Require the HIDScope library
Technical_Muffin 3:a69f041108d4 4 #include "MODSERIAL.h"
s1483080 8:5f13198a8e49 5 #include "QEI.h"
Technical_Muffin 0:1883d922ada8 6 //Define objects
Technical_Muffin 9:ba7294959988 7 AnalogIn emg1(A0); //Analog of EMG input
Technical_Muffin 9:ba7294959988 8 AnalogIn emg2(A1);
Technical_Muffin 9:ba7294959988 9 Ticker sample_timer1;
Technical_Muffin 9:ba7294959988 10 Ticker sample_timer2;
Technical_Muffin 9:ba7294959988 11 Ticker motor_timer1;
Technical_Muffin 9:ba7294959988 12 Ticker motor_timer2;
Technical_Muffin 6:ec965bb75d40 13 Ticker cal_timer;
Technical_Muffin 12:86b3885a6308 14 HIDScope scope(5); // Instantize a 2-channel HIDScope object
Technical_Muffin 2:83659da3e5fe 15 DigitalIn button1(PTA4);//test button for starting motor 1
Technical_Muffin 2:83659da3e5fe 16 DigitalOut led1(LED_RED);
Technical_Muffin 3:a69f041108d4 17 DigitalOut led2(LED_BLUE);
s1483080 8:5f13198a8e49 18 DigitalOut led3(LED_GREEN);
Technical_Muffin 3:a69f041108d4 19 MODSERIAL pc(USBTX,USBRX);
s1483080 8:5f13198a8e49 20 // motor objects
s1483080 8:5f13198a8e49 21 QEI motor1(D13,D12,NC, 624);//encoder for motor 1
s1483080 8:5f13198a8e49 22 QEI motor2(D11,D10,NC, 624);//encoder for motor 2
s1483080 8:5f13198a8e49 23 DigitalOut direction1(D7);//direction input for motor 1
s1483080 8:5f13198a8e49 24 DigitalOut direction2(D4);//direction input for motor 2
s1483080 8:5f13198a8e49 25 PwmOut speed1(D6);//speed input for motor 1
s1483080 8:5f13198a8e49 26 PwmOut speed2(D5);//speed input for motor 2
s1483080 8:5f13198a8e49 27
Technical_Muffin 0:1883d922ada8 28 /*The biquad filters required to transform the EMG signal into an usable signal*/
Technical_Muffin 11:8196434745b4 29 biquadFilter filterhigh1(-1.4542, 0.5741, 0.7571, -1.5142, 0.7571);
Technical_Muffin 11:8196434745b4 30 biquadFilter filterlow1(0.4395, 0.2059, 0.4114, 0.8227, 0.4114);
Technical_Muffin 11:8196434745b4 31 biquadFilter notch(-1.0958, 0.9723, 0.9862, -1.0958, 0.9862);
Technical_Muffin 12:86b3885a6308 32 biquadFilter filterlow2(-1.9722, 0.9726, 9.50600294492288e-05, 0.000190120058898458, 9.50600294492288e-05);
Technical_Muffin 12:86b3885a6308 33
Technical_Muffin 12:86b3885a6308 34 biquadFilter filterhigh12(-1.4542, 0.5741, 0.7571, -1.5142, 0.7571);
Technical_Muffin 12:86b3885a6308 35 biquadFilter filterlow12(0.4395, 0.2059, 0.4114, 0.8227, 0.4114);
Technical_Muffin 12:86b3885a6308 36 biquadFilter notch2(-1.0958, 0.9723, 0.9862, -1.0958, 0.9862);
Technical_Muffin 12:86b3885a6308 37 biquadFilter filterlow22(-1.9722, 0.9726, 9.50600294492288e-05, 0.000190120058898458, 9.50600294492288e-05);
Technical_Muffin 12:86b3885a6308 38
Technical_Muffin 9:ba7294959988 39 double emg_value1;
Technical_Muffin 9:ba7294959988 40 double signalpart11;
Technical_Muffin 9:ba7294959988 41 double signalpart12;
Technical_Muffin 9:ba7294959988 42 double signalpart13;
Technical_Muffin 9:ba7294959988 43 double signalpart14;
Technical_Muffin 9:ba7294959988 44 double signalfinal1;
Technical_Muffin 9:ba7294959988 45 double onoffsignal1;
Technical_Muffin 9:ba7294959988 46
Technical_Muffin 9:ba7294959988 47 double emg_value2;
Technical_Muffin 9:ba7294959988 48 double signalpart21;
Technical_Muffin 9:ba7294959988 49 double signalpart22;
Technical_Muffin 9:ba7294959988 50 double signalpart23;
Technical_Muffin 9:ba7294959988 51 double signalpart24;
Technical_Muffin 9:ba7294959988 52 double signalfinal2;
Technical_Muffin 9:ba7294959988 53 double onoffsignal2;
Technical_Muffin 9:ba7294959988 54
Technical_Muffin 12:86b3885a6308 55 double test;
Technical_Muffin 12:86b3885a6308 56
Technical_Muffin 9:ba7294959988 57
Technical_Muffin 9:ba7294959988 58 double maxcal=1;
Technical_Muffin 9:ba7294959988 59 bool calyes=1;
s1483080 8:5f13198a8e49 60 bool motor1_dir=0;//set the direction of motor 1
s1483080 8:5f13198a8e49 61 bool motor2_dir = 0;//set the direction of motor 1
s1483080 8:5f13198a8e49 62
s1483080 8:5f13198a8e49 63 float cycle = 0.3;//define the speed of the motor
s1483080 8:5f13198a8e49 64 bool motor1_on = 1;//set the on variable of motor 1
s1483080 8:5f13198a8e49 65 bool motor2_on =1;//set the on variable of motor 2
s1483080 8:5f13198a8e49 66 int n1=1;//numeric conditions to determine if the speed needs to be increased
s1483080 8:5f13198a8e49 67 int n2=1;
s1483080 8:5f13198a8e49 68
s1483080 8:5f13198a8e49 69 void changedirmotor1(){
s1483080 8:5f13198a8e49 70 motor1_dir = !motor1_dir;//code for changing direction of motor 1
s1483080 8:5f13198a8e49 71 }
s1483080 8:5f13198a8e49 72 void changedirmotor2(){
s1483080 8:5f13198a8e49 73 motor2_dir = !motor2_dir;//code for changing direction of motor 2
s1483080 8:5f13198a8e49 74 }
Technical_Muffin 6:ec965bb75d40 75
Technical_Muffin 0:1883d922ada8 76 /*
Technical_Muffin 0:1883d922ada8 77 */
Technical_Muffin 9:ba7294959988 78 void filter1(){
Technical_Muffin 6:ec965bb75d40 79 if(calyes==1){
Technical_Muffin 9:ba7294959988 80 emg_value1 = emg1.read();//read the emg value from the elektrodes
Technical_Muffin 9:ba7294959988 81 signalpart11 = notch.step(emg_value1);//Highpass filter for removing offset and artifacts
Technical_Muffin 9:ba7294959988 82 signalpart12 = filterhigh1.step(signalpart11);//rectify the filtered signal
Technical_Muffin 12:86b3885a6308 83 signalpart13 = fabs(signalpart12);//low pass filter to envelope the emg
Technical_Muffin 9:ba7294959988 84 signalpart14 = filterlow1.step(signalpart13);//notch filter to remove 50Hz signal
Technical_Muffin 9:ba7294959988 85 signalfinal1 = filterlow2.step(signalpart14);//2nd low pass filter to envelope the emg
Technical_Muffin 9:ba7294959988 86 onoffsignal1=signalfinal1/maxcal;//divide the emg signal by the max EMG to calibrate the signal per person
Technical_Muffin 11:8196434745b4 87 //pc.printf("the emg 1 signal is: %f \n",onoffsignal1);
Technical_Muffin 12:86b3885a6308 88 /*emg_value2 = emg2.read();//read the emg value from the elektrodes
Technical_Muffin 11:8196434745b4 89 signalpart21 = notch.step(emg_value2);//Highpass filter for removing offset and artifacts
Technical_Muffin 11:8196434745b4 90 signalpart22 = filterhigh1.step(signalpart21);//rectify the filtered signal
Technical_Muffin 11:8196434745b4 91 signalpart23 = abs(signalpart22);//low pass filter to envelope the emg
Technical_Muffin 11:8196434745b4 92 signalpart24 = filterlow1.step(signalpart23);//notch filter to remove 50Hz signal
Technical_Muffin 11:8196434745b4 93 signalfinal2 = filterlow2.step(signalpart24);//2nd low pass filter to envelope the emg
Technical_Muffin 12:86b3885a6308 94 onoffsignal2=signalfinal2/maxcal;*///divide the emg signal by the max EMG to calibrate the signal per person
Technical_Muffin 12:86b3885a6308 95 //test=onoffsignal2-onoffsignal1;
Technical_Muffin 9:ba7294959988 96 scope.set(0,emg_value1);//set emg signal to scope in channel 1
Technical_Muffin 9:ba7294959988 97 scope.set(1,onoffsignal1);//set filtered signal to scope in channel 2
Technical_Muffin 12:86b3885a6308 98 //scope.set(2,emg_value2);//set emg signal to scope in channel 3
Technical_Muffin 12:86b3885a6308 99 //scope.set(3,onoffsignal2);//set filtered signal to scope in channel 4
Technical_Muffin 12:86b3885a6308 100 //scope.set(4,test);
Technical_Muffin 12:86b3885a6308 101 //scope.send();//send the signals to the scope
Technical_Muffin 12:86b3885a6308 102
Technical_Muffin 12:86b3885a6308 103 //pc.printf("the difference between the 2 signals is: %f \n",test);
Technical_Muffin 11:8196434745b4 104
Technical_Muffin 6:ec965bb75d40 105 //pc.printf("emg signal %f, filtered signal %f \n",emg_value,onoffsignal);
Technical_Muffin 6:ec965bb75d40 106 }
Technical_Muffin 6:ec965bb75d40 107 }
Technical_Muffin 12:86b3885a6308 108
Technical_Muffin 12:86b3885a6308 109
Technical_Muffin 9:ba7294959988 110 void filter2(){
Technical_Muffin 9:ba7294959988 111 if(calyes==1){
Technical_Muffin 9:ba7294959988 112 emg_value2 = emg2.read();//read the emg value from the elektrodes
Technical_Muffin 12:86b3885a6308 113 signalpart21 = notch2.step(emg_value2);//Highpass filter for removing offset and artifacts
Technical_Muffin 12:86b3885a6308 114 signalpart22 = filterhigh12.step(signalpart21);//rectify the filtered signal
Technical_Muffin 12:86b3885a6308 115 signalpart23 = fabs(signalpart22);//low pass filter to envelope the emg
Technical_Muffin 12:86b3885a6308 116 signalpart24 = filterlow12.step(signalpart23);//notch filter to remove 50Hz signal
Technical_Muffin 12:86b3885a6308 117 signalfinal2 = filterlow22.step(signalpart24);//2nd low pass filter to envelope the emg
Technical_Muffin 9:ba7294959988 118 onoffsignal2=signalfinal2/maxcal;//divide the emg signal by the max EMG to calibrate the signal per person
Technical_Muffin 9:ba7294959988 119 scope.set(2,emg_value2);//set emg signal to scope in channel 1
Technical_Muffin 9:ba7294959988 120 scope.set(3,onoffsignal2);//set filtered signal to scope in channel 2
Technical_Muffin 12:86b3885a6308 121 //pc.printf("the emg 2 signal is: %f \n",onoffsignal2);
Technical_Muffin 12:86b3885a6308 122 scope.send();//send the signals to the scope
Technical_Muffin 9:ba7294959988 123 //pc.printf("emg signal %f, filtered signal %f \n",emg_value,onoffsignal);
Technical_Muffin 9:ba7294959988 124 }
Technical_Muffin 12:86b3885a6308 125 }
Technical_Muffin 12:86b3885a6308 126
Technical_Muffin 12:86b3885a6308 127
Technical_Muffin 11:8196434745b4 128
Technical_Muffin 9:ba7294959988 129 /*
Technical_Muffin 9:ba7294959988 130 void checkmotor1(){//check the normalized signal and set actions if a threshold is passed not needed in ticker move to main function
Technical_Muffin 6:ec965bb75d40 131 if(calyes==1){
Technical_Muffin 9:ba7294959988 132 if(onoffsignal1 >= 0.05){
s1483080 8:5f13198a8e49 133 led1.write(0);
s1483080 8:5f13198a8e49 134 led2.write(1);
s1483080 8:5f13198a8e49 135 while(n1 == 1){
s1483080 8:5f13198a8e49 136 changedirmotor1();
s1483080 8:5f13198a8e49 137 speed1.write(cycle);//write speed only on first run through the loop
s1483080 8:5f13198a8e49 138 direction1.write(motor1_dir);//turn motor CCW or CW
s1483080 8:5f13198a8e49 139
s1483080 8:5f13198a8e49 140 n1=0;
s1483080 8:5f13198a8e49 141 }
s1483080 8:5f13198a8e49 142 }
Technical_Muffin 9:ba7294959988 143 else if(onoffsignal1<=0.03){
Technical_Muffin 6:ec965bb75d40 144 led1.write(1);
Technical_Muffin 6:ec965bb75d40 145 led2.write(0);
s1483080 8:5f13198a8e49 146
s1483080 8:5f13198a8e49 147 while(n1==0){//check if the first run was done
s1483080 8:5f13198a8e49 148 speed1.write(0);//if so set speed to 0 and reset the run counter
s1483080 8:5f13198a8e49 149 n1=1;
Technical_Muffin 6:ec965bb75d40 150 }
s1483080 8:5f13198a8e49 151 }
s1483080 8:5f13198a8e49 152
s1483080 8:5f13198a8e49 153 }
Technical_Muffin 0:1883d922ada8 154 }
Technical_Muffin 5:46e201518dd3 155
Technical_Muffin 9:ba7294959988 156 void checkmotor2(){//check the normalized signal and set actions if a threshold is passed
Technical_Muffin 9:ba7294959988 157 if(calyes==1){
Technical_Muffin 9:ba7294959988 158 if(onoffsignal2 >= 0.05){
Technical_Muffin 9:ba7294959988 159 led1.write(0);
Technical_Muffin 9:ba7294959988 160 led2.write(1);
Technical_Muffin 9:ba7294959988 161 while(n2 == 1){
Technical_Muffin 9:ba7294959988 162 changedirmotor2();
Technical_Muffin 9:ba7294959988 163 speed2.write(cycle);//write speed only on first run through the loop
Technical_Muffin 9:ba7294959988 164 direction2.write(motor2_dir);//turn motor CCW or CW
Technical_Muffin 9:ba7294959988 165
Technical_Muffin 9:ba7294959988 166 n2=0;
Technical_Muffin 9:ba7294959988 167 }
Technical_Muffin 9:ba7294959988 168 }
Technical_Muffin 9:ba7294959988 169 else if(onoffsignal2<=0.03){
Technical_Muffin 9:ba7294959988 170 led1.write(1);
Technical_Muffin 9:ba7294959988 171 led2.write(0);
Technical_Muffin 9:ba7294959988 172
Technical_Muffin 9:ba7294959988 173 while(n2==0){//check if the first run was done
Technical_Muffin 9:ba7294959988 174 speed2.write(0);//if so set speed to 0 and reset the run counter
Technical_Muffin 9:ba7294959988 175 n2=1;
Technical_Muffin 9:ba7294959988 176 }
Technical_Muffin 9:ba7294959988 177 }
Technical_Muffin 9:ba7294959988 178
Technical_Muffin 9:ba7294959988 179 }
Technical_Muffin 9:ba7294959988 180 }
Technical_Muffin 9:ba7294959988 181
Technical_Muffin 7:87d9904c1c19 182 void calibri(){//calibration function
Technical_Muffin 6:ec965bb75d40 183 if(button1.read()==false){
Technical_Muffin 9:ba7294959988 184 for(int n =0; n<100000;n++){//read for 5000 samples as calibration
Technical_Muffin 9:ba7294959988 185 emg_value = emg1.read();//read the emg value from the electrodes
Technical_Muffin 6:ec965bb75d40 186 signalpart1 = notch.step(emg_value);//Highpass filter for removing offset and artifacts
Technical_Muffin 6:ec965bb75d40 187 signalpart2 = filterhigh1.step(signalpart1);//rectify the filtered signal
Technical_Muffin 6:ec965bb75d40 188 signalpart3 = abs(signalpart2);//low pass filter to envelope the emg
Technical_Muffin 6:ec965bb75d40 189 signalpart4 = filterlow1.step(signalpart3);//notch filter to remove 50Hz signal
Technical_Muffin 6:ec965bb75d40 190 signalfinal = filterlow2.step(signalpart4);//2nd low pass filter to envelope the emg
Technical_Muffin 6:ec965bb75d40 191 double signalmeasure = signalfinal;
Technical_Muffin 7:87d9904c1c19 192 if (signalmeasure > maxcal){//determine what the highest reachable emg signal is
Technical_Muffin 6:ec965bb75d40 193 maxcal = signalmeasure;
Technical_Muffin 6:ec965bb75d40 194 }
Technical_Muffin 6:ec965bb75d40 195 }
Technical_Muffin 6:ec965bb75d40 196 calyes=1;
Technical_Muffin 6:ec965bb75d40 197 }
Technical_Muffin 6:ec965bb75d40 198 }
Technical_Muffin 9:ba7294959988 199 */
Technical_Muffin 0:1883d922ada8 200 int main()
Technical_Muffin 0:1883d922ada8 201 {
Technical_Muffin 4:fd29407c3115 202 pc.baud(115200);
Technical_Muffin 3:a69f041108d4 203 led1.write(1);
Technical_Muffin 3:a69f041108d4 204 led2.write(1);
Technical_Muffin 9:ba7294959988 205 led3.write(1);
Technical_Muffin 9:ba7294959988 206 speed1.write(0);
Technical_Muffin 9:ba7294959988 207 speed2.write(0);
Technical_Muffin 11:8196434745b4 208 sample_timer1.attach(filter1, 0.003125);//continously execute the EMG reader and filter
Technical_Muffin 12:86b3885a6308 209 sample_timer2.attach(filter2, 0.003125);//continously execute the EMG reader and filter
Technical_Muffin 9:ba7294959988 210 //motor_timer1.attach(&checkmotor1, 0.002);//continously execute the motor controller
Technical_Muffin 9:ba7294959988 211 //motor_timer2.attach(&checkmotor2, 0.002);//continously execute the motor controller
Technical_Muffin 9:ba7294959988 212 // cal_timer.attach(&calibri, 0.002);//ticker to check if motor is being calibrated
Technical_Muffin 6:ec965bb75d40 213
Technical_Muffin 5:46e201518dd3 214
Technical_Muffin 6:ec965bb75d40 215 while(1){
Technical_Muffin 9:ba7294959988 216 if(calyes==1){
Technical_Muffin 9:ba7294959988 217 if(onoffsignal1 >= 0.05){
Technical_Muffin 9:ba7294959988 218 led1.write(0);
Technical_Muffin 9:ba7294959988 219 led2.write(1);
Technical_Muffin 9:ba7294959988 220 while(n1 == 1){
Technical_Muffin 9:ba7294959988 221 changedirmotor1();
Technical_Muffin 9:ba7294959988 222 speed1.write(cycle);//write speed only on first run through the loop
Technical_Muffin 9:ba7294959988 223 direction1.write(motor1_dir);//turn motor CCW or CW
Technical_Muffin 9:ba7294959988 224
Technical_Muffin 9:ba7294959988 225 n1=0;
Technical_Muffin 9:ba7294959988 226 }
Technical_Muffin 9:ba7294959988 227 }
Technical_Muffin 9:ba7294959988 228 else if(onoffsignal1<=0.03){
Technical_Muffin 9:ba7294959988 229 led1.write(1);
Technical_Muffin 9:ba7294959988 230 led2.write(0);
Technical_Muffin 9:ba7294959988 231
Technical_Muffin 9:ba7294959988 232 while(n1==0){//check if the first run was done
Technical_Muffin 9:ba7294959988 233 speed1.write(0);//if so set speed to 0 and reset the run counter
Technical_Muffin 9:ba7294959988 234 n1=1;
Technical_Muffin 9:ba7294959988 235 }
Technical_Muffin 9:ba7294959988 236 }
Technical_Muffin 9:ba7294959988 237
Technical_Muffin 9:ba7294959988 238 }
Technical_Muffin 9:ba7294959988 239
Technical_Muffin 9:ba7294959988 240 if(calyes==1){
Technical_Muffin 9:ba7294959988 241 if(onoffsignal2 >= 0.05){
Technical_Muffin 9:ba7294959988 242 led1.write(0);
Technical_Muffin 9:ba7294959988 243 led2.write(1);
Technical_Muffin 9:ba7294959988 244 while(n2 == 1){
Technical_Muffin 9:ba7294959988 245 changedirmotor2();
Technical_Muffin 9:ba7294959988 246 speed2.write(cycle);//write speed only on first run through the loop
Technical_Muffin 9:ba7294959988 247 direction2.write(motor2_dir);//turn motor CCW or CW
Technical_Muffin 9:ba7294959988 248
Technical_Muffin 9:ba7294959988 249 n2=0;
Technical_Muffin 9:ba7294959988 250 }
Technical_Muffin 9:ba7294959988 251 }
Technical_Muffin 9:ba7294959988 252 else if(onoffsignal2<=0.03){
Technical_Muffin 9:ba7294959988 253 led1.write(1);
Technical_Muffin 9:ba7294959988 254 led2.write(0);
Technical_Muffin 9:ba7294959988 255
Technical_Muffin 9:ba7294959988 256 while(n2==0){//check if the first run was done
Technical_Muffin 9:ba7294959988 257 speed2.write(0);//if so set speed to 0 and reset the run counter
Technical_Muffin 9:ba7294959988 258 n2=1;
Technical_Muffin 9:ba7294959988 259 }
Technical_Muffin 9:ba7294959988 260 }
Technical_Muffin 9:ba7294959988 261
Technical_Muffin 9:ba7294959988 262 }
Technical_Muffin 5:46e201518dd3 263 }
Technical_Muffin 0:1883d922ada8 264 }