![](/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:
- 32:46b18f465600
- Parent:
- 31:372ff8d49430
- Child:
- 33:3c9f8c1e9adf
diff -r 372ff8d49430 -r 46b18f465600 main.cpp --- a/main.cpp Thu May 21 15:38:12 2015 +0000 +++ b/main.cpp Fri May 22 08:35:28 2015 +0000 @@ -4,6 +4,8 @@ #include "HIDScope.h" #define P_Gain 0.995 +#define K_Gain 65 //Gain of the filtered EMG signal +#define Damp 2 //Deceleration of the motr #define tres_bi 0.05 //Biceps emg treshold #define Mass 1 // Mass value #define dt 0.002 //Sample frequency @@ -33,7 +35,7 @@ C12832_LCD lcd; //Variables for motor control -float setpoint = 6500; //Frequentie +float setpoint = 9000; //Frequentie float step_freq = 1; @@ -45,8 +47,6 @@ float lowpass_const[] = {0.0201, 0.0402 , 0.0201, 1.5610, -0.6414}; //Lowpass filter potmeter: Fc = 0.5 Hz, Fs = 500 Hz, //float lowpass_const[] = {0.000009825916403675327, 0.000019651832807350654, 0.000009825916403675327, 1.991114207740345, -0.9911535114059596}; -//lowpass for step_freq: Fc = 2 Hz, Fs = 100, Gain = 6 dB -//float lowpass1_const[] = {0.007820199259120319, 0.015640398518240638, 0.007820199259120319, 1.7347238224240125, -0.7660046194604936}; //EMG filter arm_biquad_casd_df1_inst_f32 lowpass_biceps; @@ -61,24 +61,20 @@ float lowpass_biceps_states[4]; float highnotch_biceps_states[8]; float lowpass_pot_states[4]; -float lowpass1_step_states[4]; //global variabels float filtered_biceps; float filtered_average_bi; +float filt_avg_bi_old; float filtered_pot; -float filtered_average_pot; -float filtered_step; float pot_value1_f32; -float filt_avg_bi_old; float speed_old; float acc; float force; -float spd; -float spd_old; -float D = 0; -float Damp; -float K_Gain; +float speed; +float speed_old; +float D; + void average_biceps(float filtered_biceps,float *average) { @@ -167,16 +163,14 @@ void looper_motor() { Dir = 0; - K_Gain = 20*Pot2.read(); force = K_Gain*filtered_biceps; force = force - D; acc = force/Mass; - spd = spd_old + (acc * dt); - Damp = 2*Pot1.read(); - D = spd * Damp; - step_freq = (setpoint*spd); + speed = speed_old + (acc * dt); + D = speed * Damp; + step_freq = (setpoint*speed); Step.period(1.0/step_freq); - spd_old = spd; + speed_old = speed; if (step_freq < 800) { Enable = 1; @@ -215,7 +209,7 @@ lcd.printf("Freq %.0f Hz \n", step_freq); //snelheid meting op lcd //pc.printf(" %.4f \n", Pot1.read()); //lcd.printf("filt %.3f raw %.3f \n", filtered_biceps, emg0.read()); - //pc.printf("Spd %.0f Hz p1 %.4f \n", step_freq, pot_value1_f32); //snelheid meting op lcd + //pc.printf("speed %.0f Hz p1 %.4f \n", step_freq, pot_value1_f32); //snelheid meting op lcd wait(0.01);