![](/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:
- 40:0cfd96cb25fa
- Parent:
- 39:191ae0d12bd6
- Child:
- 41:a666a531d52e
--- a/main.cpp Mon Jun 01 11:34:57 2015 +0000 +++ b/main.cpp Wed Jun 03 13:10:34 2015 +0000 @@ -3,13 +3,13 @@ #include "arm_math.h" #include "HIDScope.h" -#define K_Gain 16 //Gain of the filtered EMG signal -#define Damp 4 //Deceleration of the motor +#define K_Gain 14 //Gain of the filtered EMG signal +#define Damp 5 //Deceleration of the motor #define Mass 1 // Mass value #define dt 0.002 //Sample frequency -#define MAX_bi 0.08 //Can be used for normalisation of the EMG signal of the biceps -#define MAX_tri 0.06 -#define MIN_freq 900 //The motor turns off below this frequency +#define MAX_bi 0.09 //Can be used for normalisation of the EMG signal of the biceps +#define MAX_tri 0.09 +#define MIN_freq 500 //The motor turns off below this frequency //Motor control DigitalOut Dir(p21); @@ -38,7 +38,7 @@ C12832_LCD lcd; //Variables for motor control -float setpoint = 9000; //Frequentie setpint +float setpoint = 4400; //Frequentie setpint float step_freq = 1; //EMG filter @@ -60,13 +60,11 @@ //global variabels float filtered_biceps; float filtered_triceps; -float speed_old1; -float speed_old2; +float speed_old; float acc; float force1; float force2; -float speed1; -float speed2; +float speed; float D; void looper_emg() @@ -99,47 +97,38 @@ if (filtered_biceps > filtered_triceps) { Dir = 0; - //force2 = 0; - speed2 = 0; + force2 = 0; force1 = K_Gain*(filtered_biceps/MAX_bi); force1 = force1 - D; acc = force1/Mass; - speed1 = speed_old1 + (acc * dt); - D = speed1 * Damp; - step_freq = (setpoint*speed1); - Step.period(1.0/step_freq); - speed_old1 = speed1; + speed = speed_old + (acc * dt); + D = speed * Damp; + } else { Dir = 1; - //force1 = 0; - speed1 = 0; + force1 = 0; force2 = K_Gain*(filtered_triceps/MAX_tri); force2 = force2 - D; acc = force2/Mass; - speed2 = speed_old2 + (acc * dt); - D = speed2 * Damp; - step_freq = (setpoint*speed2); - Step.period(1.0/step_freq); - speed_old2 = speed2; - - + speed = speed_old + (acc * dt); + D = speed * Damp; } //Speed limit - /*if (speed > 1) { + if (speed > 1) { speed = 1; step_freq = setpoint; } else { step_freq = (setpoint*speed); } - Step.period(1.0/step_freq); - speed_old = speed;*/ - + - if (step_freq < MIN_freq) { + if (filtered_biceps < 0.02 && filtered_triceps < 0.02) { Enable = 1; } else { Enable = 0; } + Step.period(1.0/step_freq); + speed_old = speed; } int main() { @@ -164,7 +153,7 @@ while (1) { - lcd.printf("Freq %.0f Hz \n", step_freq); //snelheid meting op lcd + lcd.printf("Freq %.0f Hz, %.2f \n", step_freq, speed); //snelheid meting op lcd //pc.printf("%.3f \n", emg0.read()); wait(0.01); }