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:
- 80:6f9ddb8bb335
- Parent:
- 79:251d73ddbc8b
- Child:
- 81:4263d0ce34d3
--- a/main.cpp Mon Jun 22 13:13:55 2015 +0000 +++ b/main.cpp Mon Jun 22 14:47:21 2015 +0000 @@ -8,7 +8,7 @@ */ #include "mbed.h" -//#include "C12832_lcd.h" +#include "C12832_lcd.h" #include "arm_math.h" //#include "HIDScope.h" @@ -27,36 +27,36 @@ #define error_tresh 0.01 //Motor control -DigitalOut Dirx(PB_8); -PwmOut Stepx(PB_9); -DigitalOut Diry(PA_2); -PwmOut Stepy(PA_3); +DigitalOut Dirx(p21); +PwmOut Stepx(p22); +DigitalOut Diry(p23); +PwmOut Stepy(p24); //Signal to and from computer Serial pc(USBTX, USBRX); //Position sensors -AnalogIn Posx(PC_0); -AnalogIn Posy(PC_1); -DigitalOut Enablex(PB_3); -DigitalOut Enabley(PA_10); +AnalogIn Posx(p19); +AnalogIn Posy(p20); +DigitalOut Enablex(p25); +DigitalOut Enabley(p26); //Microstepping -DigitalOut MS1(PB_10); -DigitalOut MS2(PB_4); -DigitalOut MS3(PB_5); +DigitalOut MS1(p27); +DigitalOut MS2(p28); +DigitalOut MS3(p29); //EMG inputs -AnalogIn emg1(PB_0); -AnalogIn emg2(PA_4); -AnalogIn emg3(PA_1); -AnalogIn emg4(PA_0); +AnalogIn emg1(p15); +AnalogIn emg2(p16); +AnalogIn emg3(p17); +AnalogIn emg4(p18); //HIDScope scope(4); //Ticker scopeTimer; //lcd screen -//C12832_LCD lcd; +C12832_LCD lcd; //Variables for motor control float setpoint = 2000; //Frequentie setpoint @@ -144,7 +144,6 @@ void looper_motory() { - emg_y = (filtered_biceps - filtered_triceps); emg_y_abs = fabs(emg_y); force1 = emg_y_abs*K_Gain; @@ -176,7 +175,6 @@ } } - void looper_motorx() { @@ -215,7 +213,7 @@ { // 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; @@ -226,21 +224,32 @@ Enablex = 1; Enabley = 1; wait(1); - pc.printf("Start homing"); + lcd.printf("Start homing"); wait(2); - //lcd.cls(); + lcd.cls(); wait(1); Enablex = 0; Enabley = 0; + + //Homing of the motor, so you start from the same position every time. 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); - pc.printf("%.2f %.2f \n", errorx, errory); + lcd.printf("%.2f %.2f \n", Stepx.read(), Stepy.read()); + if (Ps_x < Pt_x && errorx > error_tresh) { + Dirx = 0; + 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 > Pt_y && errory > error_tresh) { Diry = 0; cy = errory * H_Gain; @@ -251,6 +260,15 @@ wait(0.01); } + if (Ps_x > Pt_x && errorx > error_tresh) { + Dirx = 1; + 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 < Pt_y && errory > error_tresh) { Diry = 1; cy = errory * H_Gain; @@ -260,28 +278,21 @@ Stepy.period(1.0/hstep_freqy); wait(0.01); } - } - pc.printf("Done"); + lcd.printf("Done"); wait(2); - //lcd.cls(); + lcd.cls(); wait(1); Enablex = 1; Enabley = 1; wait(3); - pc.printf("Start EMG Control"); + lcd.printf("Start EMG Control"); wait(2); - //lcd.cls(); + lcd.cls(); wait(1); Enablex = 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); @@ -304,15 +315,10 @@ //Microstepping control, now configured as half stepping (MS1=1,MS2=0,MS3=0) - - 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 - pc.printf("%.2f %.2f %.2f \n", Stepy.read(), step_freq1, speed1); + + lcd.printf("x %.2f, y %.2f \n", Posx.read(), Posy.read()); + //lcd.printf("%.2f %.2f %.2f %.2f \n", speed1, step_freq1, speed2, step_freq2); wait(0.01); }