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@8:5f13198a8e49, 2015-10-27 (annotated)

Committer:
s1483080
Date:
Tue Oct 27 11:56:15 2015 +0000
Revision:
8:5f13198a8e49
Parent:
7:87d9904c1c19
Child:
9:ba7294959988
EMG code with movement control only motor 1

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 0:1883d922ada8 7 AnalogIn emg(A0); //Analog of EMG input
Technical_Muffin 0:1883d922ada8 8 Ticker sample_timer;
Technical_Muffin 5:46e201518dd3 9 Ticker motor_timer;
Technical_Muffin 6:ec965bb75d40 10 Ticker cal_timer;
Technical_Muffin 0:1883d922ada8 11 HIDScope scope(2); // Instantize a 2-channel HIDScope object
Technical_Muffin 2:83659da3e5fe 12 DigitalIn button1(PTA4);//test button for starting motor 1
Technical_Muffin 2:83659da3e5fe 13 DigitalOut led1(LED_RED);
Technical_Muffin 3:a69f041108d4 14 DigitalOut led2(LED_BLUE);
s1483080 8:5f13198a8e49 15 DigitalOut led3(LED_GREEN);
Technical_Muffin 3:a69f041108d4 16 MODSERIAL pc(USBTX,USBRX);
s1483080 8:5f13198a8e49 17 // motor objects
s1483080 8:5f13198a8e49 18 QEI motor1(D13,D12,NC, 624);//encoder for motor 1
s1483080 8:5f13198a8e49 19 QEI motor2(D11,D10,NC, 624);//encoder for motor 2
s1483080 8:5f13198a8e49 20 DigitalOut direction1(D7);//direction input for motor 1
s1483080 8:5f13198a8e49 21 DigitalOut direction2(D4);//direction input for motor 2
s1483080 8:5f13198a8e49 22 PwmOut speed1(D6);//speed input for motor 1
s1483080 8:5f13198a8e49 23 PwmOut speed2(D5);//speed input for motor 2
s1483080 8:5f13198a8e49 24
Technical_Muffin 0:1883d922ada8 25 /*The biquad filters required to transform the EMG signal into an usable signal*/
Technical_Muffin 0:1883d922ada8 26 biquadFilter filterhigh1(-1.1430, 0.4128, 0.6389, -1.2779, 0.6389);
Technical_Muffin 0:1883d922ada8 27 biquadFilter filterlow1(1.9556, 0.9565, 0.9780, 1.9561, 0.9780);
Technical_Muffin 0:1883d922ada8 28 biquadFilter notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780);
Technical_Muffin 0:1883d922ada8 29 biquadFilter filterlow2(-1.9645, 0.9651, 1.5515e-4, 3.1030e-4, 1.5515e-4);
Technical_Muffin 1:9913e3886643 30 double emg_value;
Technical_Muffin 1:9913e3886643 31 double signalpart1;
Technical_Muffin 1:9913e3886643 32 double signalpart2;
Technical_Muffin 1:9913e3886643 33 double signalpart3;
Technical_Muffin 1:9913e3886643 34 double signalpart4;
Technical_Muffin 1:9913e3886643 35 double signalfinal;
Technical_Muffin 2:83659da3e5fe 36 double onoffsignal;
Technical_Muffin 4:fd29407c3115 37 double maxcal=0;
Technical_Muffin 6:ec965bb75d40 38 bool calyes=0;
s1483080 8:5f13198a8e49 39 bool motor1_dir=0;//set the direction of motor 1
s1483080 8:5f13198a8e49 40 bool motor2_dir = 0;//set the direction of motor 1
s1483080 8:5f13198a8e49 41
s1483080 8:5f13198a8e49 42 float cycle = 0.3;//define the speed of the motor
s1483080 8:5f13198a8e49 43 bool motor1_on = 1;//set the on variable of motor 1
s1483080 8:5f13198a8e49 44 bool motor2_on =1;//set the on variable of motor 2
s1483080 8:5f13198a8e49 45 int n1=1;//numeric conditions to determine if the speed needs to be increased
s1483080 8:5f13198a8e49 46 int n2=1;
s1483080 8:5f13198a8e49 47
s1483080 8:5f13198a8e49 48 void changedirmotor1(){
s1483080 8:5f13198a8e49 49 motor1_dir = !motor1_dir;//code for changing direction of motor 1
s1483080 8:5f13198a8e49 50 }
s1483080 8:5f13198a8e49 51 void changedirmotor2(){
s1483080 8:5f13198a8e49 52 motor2_dir = !motor2_dir;//code for changing direction of motor 2
s1483080 8:5f13198a8e49 53 }
Technical_Muffin 6:ec965bb75d40 54
Technical_Muffin 0:1883d922ada8 55 /*
Technical_Muffin 0:1883d922ada8 56 */
Technical_Muffin 2:83659da3e5fe 57 void filter(){
Technical_Muffin 6:ec965bb75d40 58 if(calyes==1){
Technical_Muffin 1:9913e3886643 59 emg_value = emg.read();//read the emg value from the elektrodes
Technical_Muffin 3:a69f041108d4 60 signalpart1 = notch.step(emg_value);//Highpass filter for removing offset and artifacts
Technical_Muffin 3:a69f041108d4 61 signalpart2 = filterhigh1.step(signalpart1);//rectify the filtered signal
Technical_Muffin 3:a69f041108d4 62 signalpart3 = abs(signalpart2);//low pass filter to envelope the emg
Technical_Muffin 3:a69f041108d4 63 signalpart4 = filterlow1.step(signalpart3);//notch filter to remove 50Hz signal
Technical_Muffin 1:9913e3886643 64 signalfinal = filterlow2.step(signalpart4);//2nd low pass filter to envelope the emg
Technical_Muffin 2:83659da3e5fe 65 onoffsignal=signalfinal/maxcal;//divide the emg signal by the max EMG to calibrate the signal per person
Technical_Muffin 1:9913e3886643 66 scope.set(0,emg_value);//set emg signal to scope in channel 1
Technical_Muffin 2:83659da3e5fe 67 scope.set(1,onoffsignal);//set filtered signal to scope in channel 2
Technical_Muffin 1:9913e3886643 68 scope.send();//send the signals to the scope
Technical_Muffin 6:ec965bb75d40 69 //pc.printf("emg signal %f, filtered signal %f \n",emg_value,onoffsignal);
Technical_Muffin 6:ec965bb75d40 70 }
Technical_Muffin 6:ec965bb75d40 71 }
Technical_Muffin 7:87d9904c1c19 72 void checkmotor(){//check the normalized signal and set actions if a threshold is passed
Technical_Muffin 6:ec965bb75d40 73 if(calyes==1){
s1483080 8:5f13198a8e49 74 if(onoffsignal >= 0.5){
s1483080 8:5f13198a8e49 75 led1.write(0);
s1483080 8:5f13198a8e49 76 led2.write(1);
s1483080 8:5f13198a8e49 77 while(n1 == 1){
s1483080 8:5f13198a8e49 78 changedirmotor1();
s1483080 8:5f13198a8e49 79 speed1.write(cycle);//write speed only on first run through the loop
s1483080 8:5f13198a8e49 80 direction1.write(motor1_dir);//turn motor CCW or CW
s1483080 8:5f13198a8e49 81
s1483080 8:5f13198a8e49 82 n1=0;
s1483080 8:5f13198a8e49 83 }
s1483080 8:5f13198a8e49 84 }
s1483080 8:5f13198a8e49 85 else if(onoffsignal<=0.25){
Technical_Muffin 6:ec965bb75d40 86 led1.write(1);
Technical_Muffin 6:ec965bb75d40 87 led2.write(0);
s1483080 8:5f13198a8e49 88
s1483080 8:5f13198a8e49 89 while(n1==0){//check if the first run was done
s1483080 8:5f13198a8e49 90 speed1.write(0);//if so set speed to 0 and reset the run counter
s1483080 8:5f13198a8e49 91 n1=1;
Technical_Muffin 6:ec965bb75d40 92 }
s1483080 8:5f13198a8e49 93 }
s1483080 8:5f13198a8e49 94
s1483080 8:5f13198a8e49 95 }
Technical_Muffin 0:1883d922ada8 96 }
Technical_Muffin 5:46e201518dd3 97
Technical_Muffin 7:87d9904c1c19 98 void calibri(){//calibration function
Technical_Muffin 6:ec965bb75d40 99 if(button1.read()==false){
Technical_Muffin 7:87d9904c1c19 100 for(int n =0; n<5000;n++){//read for 5000 samples as calibration
s1483080 8:5f13198a8e49 101 emg_value = emg.read();//read the emg value from the electrodes
Technical_Muffin 6:ec965bb75d40 102 signalpart1 = notch.step(emg_value);//Highpass filter for removing offset and artifacts
Technical_Muffin 6:ec965bb75d40 103 signalpart2 = filterhigh1.step(signalpart1);//rectify the filtered signal
Technical_Muffin 6:ec965bb75d40 104 signalpart3 = abs(signalpart2);//low pass filter to envelope the emg
Technical_Muffin 6:ec965bb75d40 105 signalpart4 = filterlow1.step(signalpart3);//notch filter to remove 50Hz signal
Technical_Muffin 6:ec965bb75d40 106 signalfinal = filterlow2.step(signalpart4);//2nd low pass filter to envelope the emg
Technical_Muffin 6:ec965bb75d40 107 double signalmeasure = signalfinal;
Technical_Muffin 7:87d9904c1c19 108 if (signalmeasure > maxcal){//determine what the highest reachable emg signal is
Technical_Muffin 6:ec965bb75d40 109 maxcal = signalmeasure;
Technical_Muffin 6:ec965bb75d40 110 }
Technical_Muffin 6:ec965bb75d40 111 }
Technical_Muffin 6:ec965bb75d40 112 calyes=1;
Technical_Muffin 6:ec965bb75d40 113 }
Technical_Muffin 6:ec965bb75d40 114 }
Technical_Muffin 0:1883d922ada8 115 int main()
Technical_Muffin 0:1883d922ada8 116 {
Technical_Muffin 4:fd29407c3115 117 pc.baud(115200);
Technical_Muffin 3:a69f041108d4 118 led1.write(1);
Technical_Muffin 3:a69f041108d4 119 led2.write(1);
Technical_Muffin 6:ec965bb75d40 120
Technical_Muffin 4:fd29407c3115 121 sample_timer.attach(&filter, 0.002);//continously execute the EMG reader and filter
Technical_Muffin 6:ec965bb75d40 122 motor_timer.attach(&checkmotor, 0.002);//continously execute the motor controller
Technical_Muffin 6:ec965bb75d40 123 cal_timer.attach(&calibri, 0.002);//ticker to check if motor is being calibrated
Technical_Muffin 6:ec965bb75d40 124
Technical_Muffin 5:46e201518dd3 125
Technical_Muffin 6:ec965bb75d40 126 while(1){
Technical_Muffin 7:87d9904c1c19 127 //random while loop to keep system going
Technical_Muffin 5:46e201518dd3 128 }
Technical_Muffin 0:1883d922ada8 129 }