4 directional EMG control of the XY table. Made during my bachelor end assignment.
Dependencies: C12832_lcd HIDScope mbed-dsp mbed
Revision 46:7a7cb589579a, committed 2015-06-05
- Comitter:
- jessekaiser
- Date:
- Fri Jun 05 12:33:13 2015 +0000
- Parent:
- 45:f5d74c7f8fbf
- Child:
- 47:150924ff4b2c
- Commit message:
- Code voor controle met 2 motoren en 4 emg signalen.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Jun 05 09:22:43 2015 +0000 +++ b/main.cpp Fri Jun 05 12:33:13 2015 +0000 @@ -1,3 +1,12 @@ +/*Code by Jesse Kaiser, s1355783 for control of the 2DOF Planar Table +Some variables are also numbered at the end. The numbers stands for the muscle that controls it. +Biceps = 1 +Triceps = 2 +Pectoralis Major = 3 +Deltoid = 4 +The "x" and "y" at the end of variables stand for the X-Spindle or Y-Spindle respectivly. +*/ + #include "mbed.h" #include "C12832_lcd.h" #include "arm_math.h" @@ -9,17 +18,22 @@ #define dt 0.002 //Sample frequency #define MAX_bi 0.09 //Can be used for normalisation of the EMG signal of the biceps #define MAX_tri 0.09 +#define MAX_pect 0.09 +#define MAX_delt 0.09 #define MIN_freq 500 //The motor turns off below this frequency #define EMG_tresh 0.02 //Motor control -DigitalOut Dir(p21); -PwmOut Step(p22); +DigitalOut Dirx(p12); +DigitalOut Diry(p13); +PwmOut Stepx(p21); +PwmOut Stepy(p22); //Signal to and from computer Serial pc(USBTX, USBRX); -DigitalOut Enable(p25); +DigitalOut Enablex(p24); //Connected to green led +DigitalOut Enabley(p25); //Connected to blue led //Microstepping DigitalOut MS1(p27); @@ -30,9 +44,12 @@ AnalogIn Pot1(p19); AnalogIn Pot2(p20); -AnalogIn emg1(p17); //EMG bordje bovenop, biceps -AnalogIn emg2(p15); //triceps -HIDScope scope(2); +AnalogIn emg1(p15); //EMG bordje bovenop, biceps +AnalogIn emg2(p16); //triceps +AnalogIn emg3(p17); +AnalogIn emg4(p18); + +HIDScope scope(4); Ticker scopeTimer; //lcd @@ -42,14 +59,20 @@ float setpoint = 4400; //Frequentie setpint float step_freq1 = 1; float step_freq2 = 1; +float step_freq3 = 1; +float step_freq4 = 1; //EMG filter arm_biquad_casd_df1_inst_f32 lowpass_biceps; arm_biquad_casd_df1_inst_f32 lowpass_triceps; +arm_biquad_casd_df1_inst_f32 lowpass_pect; +arm_biquad_casd_df1_inst_f32 lowpass_deltoid; //lowpass filter settings: Fc = 2 Hz, Fs = 500 Hz, Gain = -3 dB float lowpass_const[] = {0.00015514839749793376, 0.00031029679499586753, 0.00015514839749793376, 1.9644602512795832, -0.9650808448695751}; arm_biquad_casd_df1_inst_f32 highnotch_biceps; arm_biquad_casd_df1_inst_f32 highnotch_triceps; +arm_biquad_casd_df1_inst_f32 highnotch_pect; +arm_biquad_casd_df1_inst_f32 highnotch_deltoid; //highpass filter settings: Fc = 20 Hz, Fs = 500 Hz, notch Fc = 50, Fs = 500 Hz float highnotch_const[] = {0.8370879899975344, -1.6741759799950688, 0.8370879899975344, 1.6474576182593796, -0.7008943417307579, 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533}; @@ -58,47 +81,55 @@ float highnotch_biceps_states[8]; float lowpass_triceps_states[4]; float highnotch_triceps_states[8]; +float lowpass_pect_states[4]; +float highnotch_pect_states[8]; +float lowpass_deltoid_states[4]; +float highnotch_deltoid_states[8]; //global variabels -float filtered_biceps; -float filtered_triceps; -float speed_old1; -float speed_old2; -float acc1; -float acc2; -float force1; -float force2; -float speed1; -float speed2; -float damping1; -float damping2; +float filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid; +float speed_old1, speed_old2, speed_old3, speed_old4; +float acc1, acc2, acc3, acc4; +float force1, force2, force3, force4; +float speed1, speed2, speed3, speed4; +float damping1, damping2, damping3, damping4; void looper_emg() { - - - float emg_value1_f32; + float emg_value1_f32, emg_value2_f32, emg_value3_f32, emg_value4_f32; emg_value1_f32 = emg1.read(); - - float emg_value2_f32; emg_value2_f32 = emg2.read(); + emg_value3_f32 = emg3.read(); + emg_value4_f32 = emg4.read(); //process emg biceps - arm_biquad_cascade_df1_f32(&highnotch_biceps, &emg_value1_f32, &filtered_biceps, 1 ); - filtered_biceps = fabs(filtered_biceps); - arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_biceps, &filtered_biceps, 1 ); + arm_biquad_cascade_df1_f32(&highnotch_biceps, &emg_value1_f32, &filtered_biceps, 1 ); //High pass and notch filter + filtered_biceps = fabs(filtered_biceps); //Rectifier + arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_biceps, &filtered_biceps, 1 ); //low pass filter //process emg triceps arm_biquad_cascade_df1_f32(&highnotch_triceps, &emg_value2_f32, &filtered_triceps, 1 ); filtered_triceps = fabs(filtered_triceps); arm_biquad_cascade_df1_f32(&lowpass_triceps, &filtered_triceps, &filtered_triceps, 1 ); + //process emg triceps + arm_biquad_cascade_df1_f32(&highnotch_pect, &emg_value3_f32, &filtered_pect, 1 ); + filtered_pect = fabs(filtered_pect); + arm_biquad_cascade_df1_f32(&lowpass_pect, &filtered_pect, &filtered_pect, 1 ); + + //process emg triceps + arm_biquad_cascade_df1_f32(&highnotch_deltoid, &emg_value4_f32, &filtered_deltoid, 1 ); + filtered_deltoid = fabs(filtered_deltoid); + arm_biquad_cascade_df1_f32(&lowpass_deltoid, &filtered_deltoid, &filtered_deltoid, 1 ); + /*send value to PC. */ scope.set(0,filtered_biceps); //Filtered EMG signal scope.set(1,filtered_triceps); + scope.set(2,filtered_pect); + scope.set(3,filtered_deltoid); } -void looper_motor() +void looper_motory() { //Vooruit force1 = K_Gain*(filtered_biceps/MAX_bi); @@ -118,15 +149,15 @@ step_freq2 = (setpoint*speed2); speed_old2 = speed2; if (filtered_biceps > filtered_triceps) { - Dir = 0; + Diry = 0; speed2 = 0.01; speed_old2 = 0.01; - Step.period(1.0/step_freq1); + Stepy.period(1.0/step_freq1); } if (filtered_triceps > filtered_biceps) { - Dir = 1; + Diry = 1; speed1 = 0.01; speed_old1 = 0.01; - Step.period(1.0/step_freq2); + Stepy.period(1.0/step_freq2); } //Speed limit if (speed1 > 1) { @@ -139,42 +170,102 @@ } //EMG treshold if (filtered_biceps < EMG_tresh && filtered_triceps < EMG_tresh) { - Enable = 1; //Enable = 1 turns the motor off. + Enabley = 1; //Enable = 1 turns the motor off. speed1 = 0.01; speed_old1 = 0.01; speed2 = 0.01; speed_old2 = 0.01; } else { - Enable = 0; + Enabley = 0; } } +void looper_motorx() +{ + //To the left + force3 = K_Gain*(filtered_pect/MAX_pect); + force3 = force3 - damping3; + acc3 = force3/Mass; + speed3 = speed_old3 + (acc3 * dt); + damping3 = speed3 * Damp; + step_freq3 = (setpoint*speed3); + speed_old3 = speed3; + + //To the right + force4 = K_Gain*(filtered_deltoid/MAX_delt); + force4 = force4 - damping4; + acc4 = force4/Mass; + speed4 = speed_old4 + (acc4 * dt); + damping4 = speed4 * Damp; + step_freq4 = (setpoint*speed4); + speed_old4 = speed4; + if (filtered_pect > filtered_deltoid) { + Dirx = 0; + speed4 = 0.01; + speed_old4 = 0.01; + Stepy.period(1.0/step_freq3); + } if (filtered_triceps > filtered_biceps) { + Dirx = 1; + speed3 = 0.01; + speed_old3 = 0.01; + Stepy.period(1.0/step_freq4); + } + //Speed limit + if (speed3 > 1) { + speed3 = 1; + step_freq3 = setpoint; + } + if (speed4 > 1) { + speed4 = 1; + step_freq4 = setpoint; + } + //EMG treshold + if (filtered_pect < EMG_tresh && filtered_deltoid < EMG_tresh) { + Enablex = 1; //Enable = 1 turns the motor off. + speed3 = 0.01; + speed_old3 = 0.01; + speed4 = 0.01; + speed_old4 = 0.01; + } else { + Enablex = 0; + } + +} int main() { // Attach the HIDScope::send method from the scope object to the timer at 500Hz. Hier wordt de sample freq aangegeven. scopeTimer.attach_us(&scope, &HIDScope::send, 2e3); - Ticker emgtimer; + Ticker emgtimer; //biceps arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1 , lowpass_const, lowpass_biceps_states); arm_biquad_cascade_df1_init_f32(&highnotch_biceps, 2 , highnotch_const, highnotch_biceps_states); + //triceps arm_biquad_cascade_df1_init_f32(&lowpass_triceps, 1 , lowpass_const, lowpass_triceps_states); arm_biquad_cascade_df1_init_f32(&highnotch_triceps, 2 , highnotch_const, highnotch_triceps_states); + //pectoralis major + arm_biquad_cascade_df1_init_f32(&lowpass_pect, 1 , lowpass_const, lowpass_pect_states); + arm_biquad_cascade_df1_init_f32(&highnotch_pect, 2 , highnotch_const, highnotch_pect_states); + //deltoid + arm_biquad_cascade_df1_init_f32(&lowpass_deltoid, 1 , lowpass_const, lowpass_deltoid_states); + arm_biquad_cascade_df1_init_f32(&highnotch_deltoid, 2 , highnotch_const, highnotch_deltoid_states); emgtimer.attach(looper_emg, 0.002); Ticker looptimer; - looptimer.attach(looper_motor, 0.01); //Uitzoeken waarom deze frequentie! - - //Microstepping control + looptimer.attach(looper_motorx, 0.01); //X-Spindle motor, why this freq? + looptimer.attach(looper_motory, 0.01); //Y-Spindle motor + + //Microstepping control, now configured as half stepping (MS1=1,MS2=0,MS3=0) MS1 = 1; MS2 = 0; MS3 = 0; - Step.write(0.5); // Duty cycle van 50% + Stepx.write(0.5); // Duty cycle of 50% + Stepy.write(0.5); while (1) { - //lcd.printf("Bi %.2f ,Tri %.2f \n", filtered_biceps, filtered_triceps); //snelheid meting op lcd - lcd.printf("1 %.0f, 2 %.0f \n", step_freq1, step_freq2); + //lcd.printf("Bi %.2f ,Tri %.2f \n", filtered_biceps, filtered_triceps); Filtered EMG values + lcd.printf("1 %.0f, 2 %.0f, 3 %.0f, 4 %.0f \n", step_freq1, step_freq2, step_freq3, step_freq4); //step_freq value of every EMG sensor wait(0.01); } }