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@10:4af887af3a47, 2015-10-28 (annotated)

Committer:
Technical_Muffin
Date:
Wed Oct 28 11:02:52 2015 +0000
Revision:
10:4af887af3a47
Parent:
9:ba7294959988
Child:
11:8196434745b4
Code works for 1 signal, together there is conflict

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 9:ba7294959988 14 HIDScope scope(4); // 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 0:1883d922ada8 29 biquadFilter filterhigh1(-1.1430, 0.4128, 0.6389, -1.2779, 0.6389);
Technical_Muffin 0:1883d922ada8 30 biquadFilter filterlow1(1.9556, 0.9565, 0.9780, 1.9561, 0.9780);
Technical_Muffin 0:1883d922ada8 31 biquadFilter notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780);
Technical_Muffin 0:1883d922ada8 32 biquadFilter filterlow2(-1.9645, 0.9651, 1.5515e-4, 3.1030e-4, 1.5515e-4);
Technical_Muffin 9:ba7294959988 33 double emg_value1;
Technical_Muffin 9:ba7294959988 34 double signalpart11;
Technical_Muffin 9:ba7294959988 35 double signalpart12;
Technical_Muffin 9:ba7294959988 36 double signalpart13;
Technical_Muffin 9:ba7294959988 37 double signalpart14;
Technical_Muffin 9:ba7294959988 38 double signalfinal1;
Technical_Muffin 9:ba7294959988 39 double onoffsignal1;
Technical_Muffin 9:ba7294959988 40
Technical_Muffin 9:ba7294959988 41 double emg_value2;
Technical_Muffin 9:ba7294959988 42 double signalpart21;
Technical_Muffin 9:ba7294959988 43 double signalpart22;
Technical_Muffin 9:ba7294959988 44 double signalpart23;
Technical_Muffin 9:ba7294959988 45 double signalpart24;
Technical_Muffin 9:ba7294959988 46 double signalfinal2;
Technical_Muffin 9:ba7294959988 47 double onoffsignal2;
Technical_Muffin 9:ba7294959988 48
Technical_Muffin 9:ba7294959988 49
Technical_Muffin 9:ba7294959988 50 double maxcal=1;
Technical_Muffin 9:ba7294959988 51 bool calyes=1;
s1483080 8:5f13198a8e49 52 bool motor1_dir=0;//set the direction of motor 1
s1483080 8:5f13198a8e49 53 bool motor2_dir = 0;//set the direction of motor 1
s1483080 8:5f13198a8e49 54
s1483080 8:5f13198a8e49 55 float cycle = 0.3;//define the speed of the motor
s1483080 8:5f13198a8e49 56 bool motor1_on = 1;//set the on variable of motor 1
s1483080 8:5f13198a8e49 57 bool motor2_on =1;//set the on variable of motor 2
s1483080 8:5f13198a8e49 58 int n1=1;//numeric conditions to determine if the speed needs to be increased
s1483080 8:5f13198a8e49 59 int n2=1;
s1483080 8:5f13198a8e49 60
s1483080 8:5f13198a8e49 61 void changedirmotor1(){
s1483080 8:5f13198a8e49 62 motor1_dir = !motor1_dir;//code for changing direction of motor 1
s1483080 8:5f13198a8e49 63 }
s1483080 8:5f13198a8e49 64 void changedirmotor2(){
s1483080 8:5f13198a8e49 65 motor2_dir = !motor2_dir;//code for changing direction of motor 2
s1483080 8:5f13198a8e49 66 }
Technical_Muffin 6:ec965bb75d40 67
Technical_Muffin 0:1883d922ada8 68 /*
Technical_Muffin 0:1883d922ada8 69 */
Technical_Muffin 9:ba7294959988 70 void filter1(){
Technical_Muffin 6:ec965bb75d40 71 if(calyes==1){
Technical_Muffin 9:ba7294959988 72 emg_value1 = emg1.read();//read the emg value from the elektrodes
Technical_Muffin 9:ba7294959988 73 signalpart11 = notch.step(emg_value1);//Highpass filter for removing offset and artifacts
Technical_Muffin 9:ba7294959988 74 signalpart12 = filterhigh1.step(signalpart11);//rectify the filtered signal
Technical_Muffin 9:ba7294959988 75 signalpart13 = abs(signalpart12);//low pass filter to envelope the emg
Technical_Muffin 9:ba7294959988 76 signalpart14 = filterlow1.step(signalpart13);//notch filter to remove 50Hz signal
Technical_Muffin 9:ba7294959988 77 signalfinal1 = filterlow2.step(signalpart14);//2nd low pass filter to envelope the emg
Technical_Muffin 9:ba7294959988 78 onoffsignal1=signalfinal1/maxcal;//divide the emg signal by the max EMG to calibrate the signal per person
Technical_Muffin 9:ba7294959988 79 scope.set(0,emg_value1);//set emg signal to scope in channel 1
Technical_Muffin 9:ba7294959988 80 scope.set(1,onoffsignal1);//set filtered signal to scope in channel 2
Technical_Muffin 10:4af887af3a47 81 pc.printf("the emg 1 signal is: %f \n",onoffsignal1);
Technical_Muffin 1:9913e3886643 82 scope.send();//send the signals to the scope
Technical_Muffin 6:ec965bb75d40 83 //pc.printf("emg signal %f, filtered signal %f \n",emg_value,onoffsignal);
Technical_Muffin 6:ec965bb75d40 84 }
Technical_Muffin 6:ec965bb75d40 85 }
Technical_Muffin 9:ba7294959988 86
Technical_Muffin 9:ba7294959988 87 void filter2(){
Technical_Muffin 9:ba7294959988 88 if(calyes==1){
Technical_Muffin 9:ba7294959988 89 emg_value2 = emg2.read();//read the emg value from the elektrodes
Technical_Muffin 9:ba7294959988 90 signalpart21 = notch.step(emg_value2);//Highpass filter for removing offset and artifacts
Technical_Muffin 9:ba7294959988 91 signalpart22 = filterhigh1.step(signalpart21);//rectify the filtered signal
Technical_Muffin 9:ba7294959988 92 signalpart23 = abs(signalpart22);//low pass filter to envelope the emg
Technical_Muffin 9:ba7294959988 93 signalpart24 = filterlow1.step(signalpart23);//notch filter to remove 50Hz signal
Technical_Muffin 9:ba7294959988 94 signalfinal2 = filterlow2.step(signalpart24);//2nd low pass filter to envelope the emg
Technical_Muffin 9:ba7294959988 95 onoffsignal2=signalfinal2/maxcal;//divide the emg signal by the max EMG to calibrate the signal per person
Technical_Muffin 9:ba7294959988 96 scope.set(2,emg_value2);//set emg signal to scope in channel 1
Technical_Muffin 9:ba7294959988 97 scope.set(3,onoffsignal2);//set filtered signal to scope in channel 2
Technical_Muffin 10:4af887af3a47 98 pc.printf("the emg 2 signal is: %f \n",onoffsignal2);
Technical_Muffin 9:ba7294959988 99 scope.send();//send the signals to the scope
Technical_Muffin 9:ba7294959988 100 //pc.printf("emg signal %f, filtered signal %f \n",emg_value,onoffsignal);
Technical_Muffin 9:ba7294959988 101 }
Technical_Muffin 9:ba7294959988 102 }
Technical_Muffin 9:ba7294959988 103 /*
Technical_Muffin 9:ba7294959988 104 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 105 if(calyes==1){
Technical_Muffin 9:ba7294959988 106 if(onoffsignal1 >= 0.05){
s1483080 8:5f13198a8e49 107 led1.write(0);
s1483080 8:5f13198a8e49 108 led2.write(1);
s1483080 8:5f13198a8e49 109 while(n1 == 1){
s1483080 8:5f13198a8e49 110 changedirmotor1();
s1483080 8:5f13198a8e49 111 speed1.write(cycle);//write speed only on first run through the loop
s1483080 8:5f13198a8e49 112 direction1.write(motor1_dir);//turn motor CCW or CW
s1483080 8:5f13198a8e49 113
s1483080 8:5f13198a8e49 114 n1=0;
s1483080 8:5f13198a8e49 115 }
s1483080 8:5f13198a8e49 116 }
Technical_Muffin 9:ba7294959988 117 else if(onoffsignal1<=0.03){
Technical_Muffin 6:ec965bb75d40 118 led1.write(1);
Technical_Muffin 6:ec965bb75d40 119 led2.write(0);
s1483080 8:5f13198a8e49 120
s1483080 8:5f13198a8e49 121 while(n1==0){//check if the first run was done
s1483080 8:5f13198a8e49 122 speed1.write(0);//if so set speed to 0 and reset the run counter
s1483080 8:5f13198a8e49 123 n1=1;
Technical_Muffin 6:ec965bb75d40 124 }
s1483080 8:5f13198a8e49 125 }
s1483080 8:5f13198a8e49 126
s1483080 8:5f13198a8e49 127 }
Technical_Muffin 0:1883d922ada8 128 }
Technical_Muffin 5:46e201518dd3 129
Technical_Muffin 9:ba7294959988 130 void checkmotor2(){//check the normalized signal and set actions if a threshold is passed
Technical_Muffin 9:ba7294959988 131 if(calyes==1){
Technical_Muffin 9:ba7294959988 132 if(onoffsignal2 >= 0.05){
Technical_Muffin 9:ba7294959988 133 led1.write(0);
Technical_Muffin 9:ba7294959988 134 led2.write(1);
Technical_Muffin 9:ba7294959988 135 while(n2 == 1){
Technical_Muffin 9:ba7294959988 136 changedirmotor2();
Technical_Muffin 9:ba7294959988 137 speed2.write(cycle);//write speed only on first run through the loop
Technical_Muffin 9:ba7294959988 138 direction2.write(motor2_dir);//turn motor CCW or CW
Technical_Muffin 9:ba7294959988 139
Technical_Muffin 9:ba7294959988 140 n2=0;
Technical_Muffin 9:ba7294959988 141 }
Technical_Muffin 9:ba7294959988 142 }
Technical_Muffin 9:ba7294959988 143 else if(onoffsignal2<=0.03){
Technical_Muffin 9:ba7294959988 144 led1.write(1);
Technical_Muffin 9:ba7294959988 145 led2.write(0);
Technical_Muffin 9:ba7294959988 146
Technical_Muffin 9:ba7294959988 147 while(n2==0){//check if the first run was done
Technical_Muffin 9:ba7294959988 148 speed2.write(0);//if so set speed to 0 and reset the run counter
Technical_Muffin 9:ba7294959988 149 n2=1;
Technical_Muffin 9:ba7294959988 150 }
Technical_Muffin 9:ba7294959988 151 }
Technical_Muffin 9:ba7294959988 152
Technical_Muffin 9:ba7294959988 153 }
Technical_Muffin 9:ba7294959988 154 }
Technical_Muffin 9:ba7294959988 155
Technical_Muffin 7:87d9904c1c19 156 void calibri(){//calibration function
Technical_Muffin 6:ec965bb75d40 157 if(button1.read()==false){
Technical_Muffin 9:ba7294959988 158 for(int n =0; n<100000;n++){//read for 5000 samples as calibration
Technical_Muffin 9:ba7294959988 159 emg_value = emg1.read();//read the emg value from the electrodes
Technical_Muffin 6:ec965bb75d40 160 signalpart1 = notch.step(emg_value);//Highpass filter for removing offset and artifacts
Technical_Muffin 6:ec965bb75d40 161 signalpart2 = filterhigh1.step(signalpart1);//rectify the filtered signal
Technical_Muffin 6:ec965bb75d40 162 signalpart3 = abs(signalpart2);//low pass filter to envelope the emg
Technical_Muffin 6:ec965bb75d40 163 signalpart4 = filterlow1.step(signalpart3);//notch filter to remove 50Hz signal
Technical_Muffin 6:ec965bb75d40 164 signalfinal = filterlow2.step(signalpart4);//2nd low pass filter to envelope the emg
Technical_Muffin 6:ec965bb75d40 165 double signalmeasure = signalfinal;
Technical_Muffin 7:87d9904c1c19 166 if (signalmeasure > maxcal){//determine what the highest reachable emg signal is
Technical_Muffin 6:ec965bb75d40 167 maxcal = signalmeasure;
Technical_Muffin 6:ec965bb75d40 168 }
Technical_Muffin 6:ec965bb75d40 169 }
Technical_Muffin 6:ec965bb75d40 170 calyes=1;
Technical_Muffin 6:ec965bb75d40 171 }
Technical_Muffin 6:ec965bb75d40 172 }
Technical_Muffin 9:ba7294959988 173 */
Technical_Muffin 0:1883d922ada8 174 int main()
Technical_Muffin 0:1883d922ada8 175 {
Technical_Muffin 4:fd29407c3115 176 pc.baud(115200);
Technical_Muffin 3:a69f041108d4 177 led1.write(1);
Technical_Muffin 3:a69f041108d4 178 led2.write(1);
Technical_Muffin 9:ba7294959988 179 led3.write(1);
Technical_Muffin 9:ba7294959988 180 speed1.write(0);
Technical_Muffin 9:ba7294959988 181 speed2.write(0);
Technical_Muffin 10:4af887af3a47 182 sample_timer1.attach(&filter1, 0.005);//continously execute the EMG reader and filter
Technical_Muffin 10:4af887af3a47 183 // sample_timer2.attach(&filter2, 0.005);//continously execute the EMG reader and filter
Technical_Muffin 9:ba7294959988 184 //motor_timer1.attach(&checkmotor1, 0.002);//continously execute the motor controller
Technical_Muffin 9:ba7294959988 185 //motor_timer2.attach(&checkmotor2, 0.002);//continously execute the motor controller
Technical_Muffin 9:ba7294959988 186 // cal_timer.attach(&calibri, 0.002);//ticker to check if motor is being calibrated
Technical_Muffin 6:ec965bb75d40 187
Technical_Muffin 5:46e201518dd3 188
Technical_Muffin 6:ec965bb75d40 189 while(1){
Technical_Muffin 9:ba7294959988 190 if(calyes==1){
Technical_Muffin 9:ba7294959988 191 if(onoffsignal1 >= 0.05){
Technical_Muffin 9:ba7294959988 192 led1.write(0);
Technical_Muffin 9:ba7294959988 193 led2.write(1);
Technical_Muffin 9:ba7294959988 194 while(n1 == 1){
Technical_Muffin 9:ba7294959988 195 changedirmotor1();
Technical_Muffin 9:ba7294959988 196 speed1.write(cycle);//write speed only on first run through the loop
Technical_Muffin 9:ba7294959988 197 direction1.write(motor1_dir);//turn motor CCW or CW
Technical_Muffin 9:ba7294959988 198
Technical_Muffin 9:ba7294959988 199 n1=0;
Technical_Muffin 9:ba7294959988 200 }
Technical_Muffin 9:ba7294959988 201 }
Technical_Muffin 9:ba7294959988 202 else if(onoffsignal1<=0.03){
Technical_Muffin 9:ba7294959988 203 led1.write(1);
Technical_Muffin 9:ba7294959988 204 led2.write(0);
Technical_Muffin 9:ba7294959988 205
Technical_Muffin 9:ba7294959988 206 while(n1==0){//check if the first run was done
Technical_Muffin 9:ba7294959988 207 speed1.write(0);//if so set speed to 0 and reset the run counter
Technical_Muffin 9:ba7294959988 208 n1=1;
Technical_Muffin 9:ba7294959988 209 }
Technical_Muffin 9:ba7294959988 210 }
Technical_Muffin 9:ba7294959988 211
Technical_Muffin 9:ba7294959988 212 }
Technical_Muffin 9:ba7294959988 213
Technical_Muffin 9:ba7294959988 214 if(calyes==1){
Technical_Muffin 9:ba7294959988 215 if(onoffsignal2 >= 0.05){
Technical_Muffin 9:ba7294959988 216 led1.write(0);
Technical_Muffin 9:ba7294959988 217 led2.write(1);
Technical_Muffin 9:ba7294959988 218 while(n2 == 1){
Technical_Muffin 9:ba7294959988 219 changedirmotor2();
Technical_Muffin 9:ba7294959988 220 speed2.write(cycle);//write speed only on first run through the loop
Technical_Muffin 9:ba7294959988 221 direction2.write(motor2_dir);//turn motor CCW or CW
Technical_Muffin 9:ba7294959988 222
Technical_Muffin 9:ba7294959988 223 n2=0;
Technical_Muffin 9:ba7294959988 224 }
Technical_Muffin 9:ba7294959988 225 }
Technical_Muffin 9:ba7294959988 226 else if(onoffsignal2<=0.03){
Technical_Muffin 9:ba7294959988 227 led1.write(1);
Technical_Muffin 9:ba7294959988 228 led2.write(0);
Technical_Muffin 9:ba7294959988 229
Technical_Muffin 9:ba7294959988 230 while(n2==0){//check if the first run was done
Technical_Muffin 9:ba7294959988 231 speed2.write(0);//if so set speed to 0 and reset the run counter
Technical_Muffin 9:ba7294959988 232 n2=1;
Technical_Muffin 9:ba7294959988 233 }
Technical_Muffin 9:ba7294959988 234 }
Technical_Muffin 9:ba7294959988 235
Technical_Muffin 9:ba7294959988 236 }
Technical_Muffin 5:46e201518dd3 237 }
Technical_Muffin 0:1883d922ada8 238 }