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@9:ba7294959988, 2015-10-27 (annotated)

Committer:
Technical_Muffin
Date:
Tue Oct 27 22:00:46 2015 +0000
Revision:
9:ba7294959988
Parent:
8:5f13198a8e49
Child:
10:4af887af3a47
moved controller code from ticker to main to check if it doesn't overload the system. Not yet tested

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 1:9913e3886643 81 scope.send();//send the signals to the scope
Technical_Muffin 6:ec965bb75d40 82 //pc.printf("emg signal %f, filtered signal %f \n",emg_value,onoffsignal);
Technical_Muffin 6:ec965bb75d40 83 }
Technical_Muffin 6:ec965bb75d40 84 }
Technical_Muffin 9:ba7294959988 85
Technical_Muffin 9:ba7294959988 86 void filter2(){
Technical_Muffin 9:ba7294959988 87 if(calyes==1){
Technical_Muffin 9:ba7294959988 88 emg_value2 = emg2.read();//read the emg value from the elektrodes
Technical_Muffin 9:ba7294959988 89 signalpart21 = notch.step(emg_value2);//Highpass filter for removing offset and artifacts
Technical_Muffin 9:ba7294959988 90 signalpart22 = filterhigh1.step(signalpart21);//rectify the filtered signal
Technical_Muffin 9:ba7294959988 91 signalpart23 = abs(signalpart22);//low pass filter to envelope the emg
Technical_Muffin 9:ba7294959988 92 signalpart24 = filterlow1.step(signalpart23);//notch filter to remove 50Hz signal
Technical_Muffin 9:ba7294959988 93 signalfinal2 = filterlow2.step(signalpart24);//2nd low pass filter to envelope the emg
Technical_Muffin 9:ba7294959988 94 onoffsignal2=signalfinal2/maxcal;//divide the emg signal by the max EMG to calibrate the signal per person
Technical_Muffin 9:ba7294959988 95 scope.set(2,emg_value2);//set emg signal to scope in channel 1
Technical_Muffin 9:ba7294959988 96 scope.set(3,onoffsignal2);//set filtered signal to scope in channel 2
Technical_Muffin 9:ba7294959988 97 scope.send();//send the signals to the scope
Technical_Muffin 9:ba7294959988 98 //pc.printf("emg signal %f, filtered signal %f \n",emg_value,onoffsignal);
Technical_Muffin 9:ba7294959988 99 }
Technical_Muffin 9:ba7294959988 100 }
Technical_Muffin 9:ba7294959988 101 /*
Technical_Muffin 9:ba7294959988 102 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 103 if(calyes==1){
Technical_Muffin 9:ba7294959988 104 if(onoffsignal1 >= 0.05){
s1483080 8:5f13198a8e49 105 led1.write(0);
s1483080 8:5f13198a8e49 106 led2.write(1);
s1483080 8:5f13198a8e49 107 while(n1 == 1){
s1483080 8:5f13198a8e49 108 changedirmotor1();
s1483080 8:5f13198a8e49 109 speed1.write(cycle);//write speed only on first run through the loop
s1483080 8:5f13198a8e49 110 direction1.write(motor1_dir);//turn motor CCW or CW
s1483080 8:5f13198a8e49 111
s1483080 8:5f13198a8e49 112 n1=0;
s1483080 8:5f13198a8e49 113 }
s1483080 8:5f13198a8e49 114 }
Technical_Muffin 9:ba7294959988 115 else if(onoffsignal1<=0.03){
Technical_Muffin 6:ec965bb75d40 116 led1.write(1);
Technical_Muffin 6:ec965bb75d40 117 led2.write(0);
s1483080 8:5f13198a8e49 118
s1483080 8:5f13198a8e49 119 while(n1==0){//check if the first run was done
s1483080 8:5f13198a8e49 120 speed1.write(0);//if so set speed to 0 and reset the run counter
s1483080 8:5f13198a8e49 121 n1=1;
Technical_Muffin 6:ec965bb75d40 122 }
s1483080 8:5f13198a8e49 123 }
s1483080 8:5f13198a8e49 124
s1483080 8:5f13198a8e49 125 }
Technical_Muffin 0:1883d922ada8 126 }
Technical_Muffin 5:46e201518dd3 127
Technical_Muffin 9:ba7294959988 128 void checkmotor2(){//check the normalized signal and set actions if a threshold is passed
Technical_Muffin 9:ba7294959988 129 if(calyes==1){
Technical_Muffin 9:ba7294959988 130 if(onoffsignal2 >= 0.05){
Technical_Muffin 9:ba7294959988 131 led1.write(0);
Technical_Muffin 9:ba7294959988 132 led2.write(1);
Technical_Muffin 9:ba7294959988 133 while(n2 == 1){
Technical_Muffin 9:ba7294959988 134 changedirmotor2();
Technical_Muffin 9:ba7294959988 135 speed2.write(cycle);//write speed only on first run through the loop
Technical_Muffin 9:ba7294959988 136 direction2.write(motor2_dir);//turn motor CCW or CW
Technical_Muffin 9:ba7294959988 137
Technical_Muffin 9:ba7294959988 138 n2=0;
Technical_Muffin 9:ba7294959988 139 }
Technical_Muffin 9:ba7294959988 140 }
Technical_Muffin 9:ba7294959988 141 else if(onoffsignal2<=0.03){
Technical_Muffin 9:ba7294959988 142 led1.write(1);
Technical_Muffin 9:ba7294959988 143 led2.write(0);
Technical_Muffin 9:ba7294959988 144
Technical_Muffin 9:ba7294959988 145 while(n2==0){//check if the first run was done
Technical_Muffin 9:ba7294959988 146 speed2.write(0);//if so set speed to 0 and reset the run counter
Technical_Muffin 9:ba7294959988 147 n2=1;
Technical_Muffin 9:ba7294959988 148 }
Technical_Muffin 9:ba7294959988 149 }
Technical_Muffin 9:ba7294959988 150
Technical_Muffin 9:ba7294959988 151 }
Technical_Muffin 9:ba7294959988 152 }
Technical_Muffin 9:ba7294959988 153
Technical_Muffin 7:87d9904c1c19 154 void calibri(){//calibration function
Technical_Muffin 6:ec965bb75d40 155 if(button1.read()==false){
Technical_Muffin 9:ba7294959988 156 for(int n =0; n<100000;n++){//read for 5000 samples as calibration
Technical_Muffin 9:ba7294959988 157 emg_value = emg1.read();//read the emg value from the electrodes
Technical_Muffin 6:ec965bb75d40 158 signalpart1 = notch.step(emg_value);//Highpass filter for removing offset and artifacts
Technical_Muffin 6:ec965bb75d40 159 signalpart2 = filterhigh1.step(signalpart1);//rectify the filtered signal
Technical_Muffin 6:ec965bb75d40 160 signalpart3 = abs(signalpart2);//low pass filter to envelope the emg
Technical_Muffin 6:ec965bb75d40 161 signalpart4 = filterlow1.step(signalpart3);//notch filter to remove 50Hz signal
Technical_Muffin 6:ec965bb75d40 162 signalfinal = filterlow2.step(signalpart4);//2nd low pass filter to envelope the emg
Technical_Muffin 6:ec965bb75d40 163 double signalmeasure = signalfinal;
Technical_Muffin 7:87d9904c1c19 164 if (signalmeasure > maxcal){//determine what the highest reachable emg signal is
Technical_Muffin 6:ec965bb75d40 165 maxcal = signalmeasure;
Technical_Muffin 6:ec965bb75d40 166 }
Technical_Muffin 6:ec965bb75d40 167 }
Technical_Muffin 6:ec965bb75d40 168 calyes=1;
Technical_Muffin 6:ec965bb75d40 169 }
Technical_Muffin 6:ec965bb75d40 170 }
Technical_Muffin 9:ba7294959988 171 */
Technical_Muffin 0:1883d922ada8 172 int main()
Technical_Muffin 0:1883d922ada8 173 {
Technical_Muffin 4:fd29407c3115 174 pc.baud(115200);
Technical_Muffin 3:a69f041108d4 175 led1.write(1);
Technical_Muffin 3:a69f041108d4 176 led2.write(1);
Technical_Muffin 9:ba7294959988 177 led3.write(1);
Technical_Muffin 9:ba7294959988 178 speed1.write(0);
Technical_Muffin 9:ba7294959988 179 speed2.write(0);
Technical_Muffin 9:ba7294959988 180 sample_timer1.attach(&filter1, 0.002);//continously execute the EMG reader and filter
Technical_Muffin 9:ba7294959988 181 sample_timer2.attach(&filter2, 0.002);//continously execute the EMG reader and filter
Technical_Muffin 9:ba7294959988 182 //motor_timer1.attach(&checkmotor1, 0.002);//continously execute the motor controller
Technical_Muffin 9:ba7294959988 183 //motor_timer2.attach(&checkmotor2, 0.002);//continously execute the motor controller
Technical_Muffin 9:ba7294959988 184 // cal_timer.attach(&calibri, 0.002);//ticker to check if motor is being calibrated
Technical_Muffin 6:ec965bb75d40 185
Technical_Muffin 5:46e201518dd3 186
Technical_Muffin 6:ec965bb75d40 187 while(1){
Technical_Muffin 9:ba7294959988 188 if(calyes==1){
Technical_Muffin 9:ba7294959988 189 if(onoffsignal1 >= 0.05){
Technical_Muffin 9:ba7294959988 190 led1.write(0);
Technical_Muffin 9:ba7294959988 191 led2.write(1);
Technical_Muffin 9:ba7294959988 192 while(n1 == 1){
Technical_Muffin 9:ba7294959988 193 changedirmotor1();
Technical_Muffin 9:ba7294959988 194 speed1.write(cycle);//write speed only on first run through the loop
Technical_Muffin 9:ba7294959988 195 direction1.write(motor1_dir);//turn motor CCW or CW
Technical_Muffin 9:ba7294959988 196
Technical_Muffin 9:ba7294959988 197 n1=0;
Technical_Muffin 9:ba7294959988 198 }
Technical_Muffin 9:ba7294959988 199 }
Technical_Muffin 9:ba7294959988 200 else if(onoffsignal1<=0.03){
Technical_Muffin 9:ba7294959988 201 led1.write(1);
Technical_Muffin 9:ba7294959988 202 led2.write(0);
Technical_Muffin 9:ba7294959988 203
Technical_Muffin 9:ba7294959988 204 while(n1==0){//check if the first run was done
Technical_Muffin 9:ba7294959988 205 speed1.write(0);//if so set speed to 0 and reset the run counter
Technical_Muffin 9:ba7294959988 206 n1=1;
Technical_Muffin 9:ba7294959988 207 }
Technical_Muffin 9:ba7294959988 208 }
Technical_Muffin 9:ba7294959988 209
Technical_Muffin 9:ba7294959988 210 }
Technical_Muffin 9:ba7294959988 211
Technical_Muffin 9:ba7294959988 212 if(calyes==1){
Technical_Muffin 9:ba7294959988 213 if(onoffsignal2 >= 0.05){
Technical_Muffin 9:ba7294959988 214 led1.write(0);
Technical_Muffin 9:ba7294959988 215 led2.write(1);
Technical_Muffin 9:ba7294959988 216 while(n2 == 1){
Technical_Muffin 9:ba7294959988 217 changedirmotor2();
Technical_Muffin 9:ba7294959988 218 speed2.write(cycle);//write speed only on first run through the loop
Technical_Muffin 9:ba7294959988 219 direction2.write(motor2_dir);//turn motor CCW or CW
Technical_Muffin 9:ba7294959988 220
Technical_Muffin 9:ba7294959988 221 n2=0;
Technical_Muffin 9:ba7294959988 222 }
Technical_Muffin 9:ba7294959988 223 }
Technical_Muffin 9:ba7294959988 224 else if(onoffsignal2<=0.03){
Technical_Muffin 9:ba7294959988 225 led1.write(1);
Technical_Muffin 9:ba7294959988 226 led2.write(0);
Technical_Muffin 9:ba7294959988 227
Technical_Muffin 9:ba7294959988 228 while(n2==0){//check if the first run was done
Technical_Muffin 9:ba7294959988 229 speed2.write(0);//if so set speed to 0 and reset the run counter
Technical_Muffin 9:ba7294959988 230 n2=1;
Technical_Muffin 9:ba7294959988 231 }
Technical_Muffin 9:ba7294959988 232 }
Technical_Muffin 9:ba7294959988 233
Technical_Muffin 9:ba7294959988 234 }
Technical_Muffin 5:46e201518dd3 235 }
Technical_Muffin 0:1883d922ada8 236 }