![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
4 directional EMG control of the XY table. Made during my bachelor end assignment.
Dependencies: C12832_lcd HIDScope mbed-dsp mbed
Diff: main.cpp
- Revision:
- 69:a1ba54587b35
- Parent:
- 68:2b778b6da923
- Child:
- 70:e84629c7dfed
diff -r 2b778b6da923 -r a1ba54587b35 main.cpp --- a/main.cpp Fri Jun 19 09:19:43 2015 +0000 +++ b/main.cpp Fri Jun 19 11:25:22 2015 +0000 @@ -12,7 +12,7 @@ #include "arm_math.h" //#include "HIDScope.h" -#define P_Gain 0.995 +#define P_Gain 0.99 #define K_Gain 150 //Gain of the filtered EMG signal #define Damp 5 //Deceleration of the motor #define Mass 1 // Mass value @@ -25,9 +25,10 @@ #define EMG_tresh2 0.01 #define EMG_tresh3 0.01 #define EMG_tresh4 0.01 -#define H_Gain 3 -#define Pt_x 0.88 -#define Pt_y 0.25 +#define H_Gain 3.5 +#define Pt_x 0.50 +#define Pt_y 0.50 +#define error_tresh 0.03 //Motor control DigitalOut Dirx(p21); @@ -65,8 +66,7 @@ float setpoint = 2000; //Frequentie setpoint 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; @@ -108,6 +108,8 @@ float Ps_y = 0; float hstep_freqx = 1; float hstep_freqy = 1; +float emg_y_abs = 0; +float emg_x_abs = 0; void looper_emg() { @@ -145,8 +147,8 @@ } void looper_motory() -{ float emg_y_abs; - +{ + emg_y = (filtered_biceps - filtered_triceps); emg_y_abs = fabs(emg_y); force1 = emg_y_abs*K_Gain; @@ -158,7 +160,6 @@ Stepy.period(1.0/step_freq1); speed_old1 = speed1; - if (emg_y > 0) { Diry = 1; } @@ -180,9 +181,9 @@ } -void looper_motorx() +/*void looper_motorx() { - float emg_x_abs; + emg_x = (filtered_pect - filtered_deltoid); emg_x_abs = fabs(emg_x); force2 = emg_x_abs*K_Gain; @@ -212,88 +213,87 @@ 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); - +/* MS1 = 1; MS2 = 0; MS3 = 0; - + Stepx.write(0.5); // Duty cycle of 50% Stepy.write(0.5); - + Enablex = 1; Enabley = 1; - wait(1); + wait(1); lcd.printf("Start homing"); wait(2); lcd.cls(); wait(1); Enablex = 0; Enabley = 0; - while(errorx > 0.03 || errory > 0.03) { - lcd.printf("%.0f %.2f \n", hstep_freqx, hstep_freqy ); + while(errorx > error_tresh || errory > error_tresh) { Ps_x = Posx.read(); Ps_y = Posy.read(); errorx = fabs(Pt_x - Ps_x); errory = fabs(Ps_y - Pt_y); - - if (Ps_x < 0.88 && errorx > 0.03) { + lcd.printf("%.2f %.2f \n", errorx, errory); + + + if (Ps_x < 0.50 && errorx > error_tresh) { Dirx = 0; //errorx = Pt_x - Ps_x; cx = errorx * H_Gain; - + float hnew_step_freqx; hnew_step_freqx = ((1-P_Gain)*setpoint*cx) + (P_Gain*hstep_freqx); hstep_freqx = hnew_step_freqx; Stepx.period(1.0/hstep_freqx); wait(0.01); } - if (Ps_y > 0.25 && errory > 0.03) { + if (Ps_y > 0.50 && errory > error_tresh) { Diry = 0; //errory = Ps_y - Pt_y; cy = errory * H_Gain; - + float hnew_step_freqy; hnew_step_freqy = ((1-P_Gain)*setpoint*cy) + (P_Gain*hstep_freqy); hstep_freqy = hnew_step_freqy; Stepy.period(1.0/hstep_freqy); wait(0.01); } - - if (Ps_x > 0.88 && errorx > 0.03) { + + if (Ps_x > 0.50 && errorx > error_tresh) { Dirx = 1; //errorx = Pt_x - Ps_x; cx = errorx * H_Gain; - + float hnew_step_freqx; hnew_step_freqx = ((1-P_Gain)*setpoint*cx) + (P_Gain*hstep_freqx); hstep_freqx = hnew_step_freqx; Stepx.period(1.0/hstep_freqx); wait(0.01); } - if (Ps_y < 0.25 && errory > 0.03) { + if (Ps_y < 0.50 && errory > error_tresh) { Diry = 1; //errory = Ps_y - Pt_y; cy = errory * H_Gain; - + float hnew_step_freqy; hnew_step_freqy = ((1-P_Gain)*setpoint*cy) + (P_Gain*hstep_freqy); hstep_freqy = hnew_step_freqy; Stepy.period(1.0/hstep_freqy); wait(0.01); } - + } - lcd.cls(); - wait(1); lcd.printf("Done"); - wait(1); + wait(2); lcd.cls(); wait(1); Enablex = 1; @@ -302,47 +302,48 @@ lcd.printf("Start EMG Control"); wait(2); lcd.cls(); - wait(1); + wait(1); Enablex = 0; - Enabley = 0; + Enabley = 0;*/ + + MS1 = 1; + MS2 = 0; + MS3 = 0; + Stepx.write(0.5); // Duty cycle of 50% + Stepy.write(0.5); + 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.01); + 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.01); - Ticker looptimer1; - looptimer1.attach(looper_motorx, 0.01); //X-Spindle motor, why this freq? - - Ticker looptimer2; - looptimer2.attach(looper_motory, 0.01); //Y-Spindle motor + //Ticker looptimer1; + //looptimer1.attach(looper_motorx, 0.01); //X-Spindle motor, why this freq? - //Microstepping control, now configured as half stepping (MS1=1,MS2=0,MS3=0) - MS1 = 1; - MS2 = 0; - MS3 = 0; - Stepx.write(0.5); // Duty cycle of 50% - Stepy.write(0.5); + Ticker looptimer2; + looptimer2.attach(looper_motory, 0.01); //Y-Spindle motor + + //Microstepping control, now configured as half stepping (MS1=1,MS2=0,MS3=0) - while (1) { + + while (1) { - //lcd.printf("x %.2f, y %.2f \n", Posx.read(), Posy.read()); - //lcd.printf("%.2f, %.2f %.2f %.2f \n", filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid); //Filtered EMG values - //lcd.printf("1 %.0f, 2 %.0f \n", step_freq1, step_freq2); //step_freq value of every EMG sensor - lcd.printf("%.0f %.2f \n", Stepy.read(), emg_y); - //lcd.printf("%.2f, %.2f %.2f %.2f \n", gain_biceps, gain_triceps, gain_pect, gain_deltoid); - //lcd.printf("%.2f, %.2f %.2f %.2f \n", norm_biceps, norm_triceps, norm_pect, norm_deltoid); - wait(0.01); + //lcd.printf("x %.2f, y %.2f \n", Posx.read(), Posy.read()); + //lcd.printf("%.2f, %.2f %.2f %.2f \n", filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid); //Filtered EMG values + //lcd.printf("1 %.0f, 2 %.0f \n", step_freq1, step_freq2); //step_freq value of every EMG sensor + lcd.printf("%.2f %.2f %.2f %.2f \n", emg_y_abs, step_freq1, filtered_biceps, filtered_triceps); + wait(0.01); - } + } }