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
main.cpp
- Committer:
- Technical_Muffin
- Date:
- 2015-10-28
- Revision:
- 12:86b3885a6308
- Parent:
- 11:8196434745b4
File content as of revision 12:86b3885a6308:
#include "mbed.h" #include "HIDScope.h" #include "biquadFilter.h" // Require the HIDScope library #include "MODSERIAL.h" #include "QEI.h" //Define objects AnalogIn emg1(A0); //Analog of EMG input AnalogIn emg2(A1); Ticker sample_timer1; Ticker sample_timer2; Ticker motor_timer1; Ticker motor_timer2; Ticker cal_timer; HIDScope scope(5); // Instantize a 2-channel HIDScope object DigitalIn button1(PTA4);//test button for starting motor 1 DigitalOut led1(LED_RED); DigitalOut led2(LED_BLUE); DigitalOut led3(LED_GREEN); MODSERIAL pc(USBTX,USBRX); // motor objects QEI motor1(D13,D12,NC, 624);//encoder for motor 1 QEI motor2(D11,D10,NC, 624);//encoder for motor 2 DigitalOut direction1(D7);//direction input for motor 1 DigitalOut direction2(D4);//direction input for motor 2 PwmOut speed1(D6);//speed input for motor 1 PwmOut speed2(D5);//speed input for motor 2 /*The biquad filters required to transform the EMG signal into an usable signal*/ biquadFilter filterhigh1(-1.4542, 0.5741, 0.7571, -1.5142, 0.7571); biquadFilter filterlow1(0.4395, 0.2059, 0.4114, 0.8227, 0.4114); biquadFilter notch(-1.0958, 0.9723, 0.9862, -1.0958, 0.9862); biquadFilter filterlow2(-1.9722, 0.9726, 9.50600294492288e-05, 0.000190120058898458, 9.50600294492288e-05); biquadFilter filterhigh12(-1.4542, 0.5741, 0.7571, -1.5142, 0.7571); biquadFilter filterlow12(0.4395, 0.2059, 0.4114, 0.8227, 0.4114); biquadFilter notch2(-1.0958, 0.9723, 0.9862, -1.0958, 0.9862); biquadFilter filterlow22(-1.9722, 0.9726, 9.50600294492288e-05, 0.000190120058898458, 9.50600294492288e-05); double emg_value1; double signalpart11; double signalpart12; double signalpart13; double signalpart14; double signalfinal1; double onoffsignal1; double emg_value2; double signalpart21; double signalpart22; double signalpart23; double signalpart24; double signalfinal2; double onoffsignal2; double test; double maxcal=1; bool calyes=1; bool motor1_dir=0;//set the direction of motor 1 bool motor2_dir = 0;//set the direction of motor 1 float cycle = 0.3;//define the speed of the motor bool motor1_on = 1;//set the on variable of motor 1 bool motor2_on =1;//set the on variable of motor 2 int n1=1;//numeric conditions to determine if the speed needs to be increased int n2=1; void changedirmotor1(){ motor1_dir = !motor1_dir;//code for changing direction of motor 1 } void changedirmotor2(){ motor2_dir = !motor2_dir;//code for changing direction of motor 2 } /* */ void filter1(){ if(calyes==1){ emg_value1 = emg1.read();//read the emg value from the elektrodes signalpart11 = notch.step(emg_value1);//Highpass filter for removing offset and artifacts signalpart12 = filterhigh1.step(signalpart11);//rectify the filtered signal signalpart13 = fabs(signalpart12);//low pass filter to envelope the emg signalpart14 = filterlow1.step(signalpart13);//notch filter to remove 50Hz signal signalfinal1 = filterlow2.step(signalpart14);//2nd low pass filter to envelope the emg onoffsignal1=signalfinal1/maxcal;//divide the emg signal by the max EMG to calibrate the signal per person //pc.printf("the emg 1 signal is: %f \n",onoffsignal1); /*emg_value2 = emg2.read();//read the emg value from the elektrodes signalpart21 = notch.step(emg_value2);//Highpass filter for removing offset and artifacts signalpart22 = filterhigh1.step(signalpart21);//rectify the filtered signal signalpart23 = abs(signalpart22);//low pass filter to envelope the emg signalpart24 = filterlow1.step(signalpart23);//notch filter to remove 50Hz signal signalfinal2 = filterlow2.step(signalpart24);//2nd low pass filter to envelope the emg onoffsignal2=signalfinal2/maxcal;*///divide the emg signal by the max EMG to calibrate the signal per person //test=onoffsignal2-onoffsignal1; scope.set(0,emg_value1);//set emg signal to scope in channel 1 scope.set(1,onoffsignal1);//set filtered signal to scope in channel 2 //scope.set(2,emg_value2);//set emg signal to scope in channel 3 //scope.set(3,onoffsignal2);//set filtered signal to scope in channel 4 //scope.set(4,test); //scope.send();//send the signals to the scope //pc.printf("the difference between the 2 signals is: %f \n",test); //pc.printf("emg signal %f, filtered signal %f \n",emg_value,onoffsignal); } } void filter2(){ if(calyes==1){ emg_value2 = emg2.read();//read the emg value from the elektrodes signalpart21 = notch2.step(emg_value2);//Highpass filter for removing offset and artifacts signalpart22 = filterhigh12.step(signalpart21);//rectify the filtered signal signalpart23 = fabs(signalpart22);//low pass filter to envelope the emg signalpart24 = filterlow12.step(signalpart23);//notch filter to remove 50Hz signal signalfinal2 = filterlow22.step(signalpart24);//2nd low pass filter to envelope the emg onoffsignal2=signalfinal2/maxcal;//divide the emg signal by the max EMG to calibrate the signal per person scope.set(2,emg_value2);//set emg signal to scope in channel 1 scope.set(3,onoffsignal2);//set filtered signal to scope in channel 2 //pc.printf("the emg 2 signal is: %f \n",onoffsignal2); scope.send();//send the signals to the scope //pc.printf("emg signal %f, filtered signal %f \n",emg_value,onoffsignal); } } /* void checkmotor1(){//check the normalized signal and set actions if a threshold is passed not needed in ticker move to main function if(calyes==1){ if(onoffsignal1 >= 0.05){ led1.write(0); led2.write(1); while(n1 == 1){ changedirmotor1(); speed1.write(cycle);//write speed only on first run through the loop direction1.write(motor1_dir);//turn motor CCW or CW n1=0; } } else if(onoffsignal1<=0.03){ led1.write(1); led2.write(0); while(n1==0){//check if the first run was done speed1.write(0);//if so set speed to 0 and reset the run counter n1=1; } } } } void checkmotor2(){//check the normalized signal and set actions if a threshold is passed if(calyes==1){ if(onoffsignal2 >= 0.05){ led1.write(0); led2.write(1); while(n2 == 1){ changedirmotor2(); speed2.write(cycle);//write speed only on first run through the loop direction2.write(motor2_dir);//turn motor CCW or CW n2=0; } } else if(onoffsignal2<=0.03){ led1.write(1); led2.write(0); while(n2==0){//check if the first run was done speed2.write(0);//if so set speed to 0 and reset the run counter n2=1; } } } } void calibri(){//calibration function if(button1.read()==false){ for(int n =0; n<100000;n++){//read for 5000 samples as calibration emg_value = emg1.read();//read the emg value from the electrodes signalpart1 = notch.step(emg_value);//Highpass filter for removing offset and artifacts signalpart2 = filterhigh1.step(signalpart1);//rectify the filtered signal signalpart3 = abs(signalpart2);//low pass filter to envelope the emg signalpart4 = filterlow1.step(signalpart3);//notch filter to remove 50Hz signal signalfinal = filterlow2.step(signalpart4);//2nd low pass filter to envelope the emg double signalmeasure = signalfinal; if (signalmeasure > maxcal){//determine what the highest reachable emg signal is maxcal = signalmeasure; } } calyes=1; } } */ int main() { pc.baud(115200); led1.write(1); led2.write(1); led3.write(1); speed1.write(0); speed2.write(0); sample_timer1.attach(filter1, 0.003125);//continously execute the EMG reader and filter sample_timer2.attach(filter2, 0.003125);//continously execute the EMG reader and filter //motor_timer1.attach(&checkmotor1, 0.002);//continously execute the motor controller //motor_timer2.attach(&checkmotor2, 0.002);//continously execute the motor controller // cal_timer.attach(&calibri, 0.002);//ticker to check if motor is being calibrated while(1){ if(calyes==1){ if(onoffsignal1 >= 0.05){ led1.write(0); led2.write(1); while(n1 == 1){ changedirmotor1(); speed1.write(cycle);//write speed only on first run through the loop direction1.write(motor1_dir);//turn motor CCW or CW n1=0; } } else if(onoffsignal1<=0.03){ led1.write(1); led2.write(0); while(n1==0){//check if the first run was done speed1.write(0);//if so set speed to 0 and reset the run counter n1=1; } } } if(calyes==1){ if(onoffsignal2 >= 0.05){ led1.write(0); led2.write(1); while(n2 == 1){ changedirmotor2(); speed2.write(cycle);//write speed only on first run through the loop direction2.write(motor2_dir);//turn motor CCW or CW n2=0; } } else if(onoffsignal2<=0.03){ led1.write(1); led2.write(0); while(n2==0){//check if the first run was done speed2.write(0);//if so set speed to 0 and reset the run counter n2=1; } } } } }