![](/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:
- 33:3c9f8c1e9adf
- Parent:
- 32:46b18f465600
- Child:
- 34:025b324d15d6
diff -r 46b18f465600 -r 3c9f8c1e9adf main.cpp --- a/main.cpp Fri May 22 08:35:28 2015 +0000 +++ b/main.cpp Fri May 22 08:43:27 2015 +0000 @@ -3,12 +3,12 @@ #include "arm_math.h" #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 +#define K_Gain 65 //Gain of the filtered EMG signal +#define Damp 2 //Deceleration of the motr +#define Mass 1 // Mass value +#define dt 0.002 //Sample frequency +#define MAX_emg +#define MIN_freq 900 //Motor control DigitalOut Dir(p21); @@ -56,7 +56,6 @@ //highpass filter settings: Fc = 20 Hz, Fs = 500 Hz, Gain = -3 dB, notch Fc = 50, Fs =500Hz, Gain = -3 dB float highnotch_const[] = {0.8370879899975344, -1.6741759799950688, 0.8370879899975344, 1.6474576182593796, -0.7008943417307579, 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533}; - //state values float lowpass_biceps_states[4]; float highnotch_biceps_states[8]; @@ -75,20 +74,6 @@ float speed_old; float D; - -void average_biceps(float filtered_biceps,float *average) -{ - static float total=0; - static float number=0; - total = total + filtered_biceps; - number = number + 1; - if ( number == 50) { - *average = total/50; - total = 0; - number = 0; - } -} - void looper_emg() { /*variable to store value in*/ @@ -96,9 +81,8 @@ float emg_value1_f32; - /*put raw emg value both in red and in emg_value*/ - emg_value1 = emg0.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V) + emg_value1 = emg0.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V) (KAN weg?) emg_value1_f32 = emg0.read(); //process emg biceps @@ -108,57 +92,8 @@ average_biceps(filtered_biceps,&filtered_average_bi); /*send value to PC. */ - scope.set(0,filtered_average_bi); //Raw EMG signal biceps - scope.set(1,filtered_biceps); //Filtered signal - + scope.set(0,filtered_biceps); //Filtered EMG signal } -/*void looper_pot() -{ - - pot_value1_f32 = Pot1.read() - 0.500; - - //process input - arm_biquad_cascade_df1_f32(&lowpass_pot, &pot_value1_f32, &filtered_pot, 1 ); - -}*/ - - -/*void looper_motor() -{ - float new_step_freq; - float speed; - - speed = 0.02*filtered_average_bi + 0.02*filt_avg_bi_old + 0.96*speed_old; //Value between 0 and 1 - new_step_freq = (setpoint*speed); - step_freq = abs(new_step_freq); //Gives the PWM frequenty to the motor. - speed_old = speed; - filt_avg_bi_old = filtered_average_bi; - - if (step_freq < 500 || step_freq > 8000) { - Enable = 1; - } else { - Enable = 0; - } - Step.period(1.0/(100 + step_freq)); //Step_freq is het aantal Hz. - -}*/ -//Motor accelereren met EMG treshold -/*void looper_motor() -{ - float new_step_freq; - Dir = 0; - - if (filtered_average_bi > tres_bi) { - Enable = 0; - new_step_freq = ((1-P)*setpoint) + (P*step_freq); - step_freq = new_step_freq; - Step.period(1.0/step_freq); - } else { - Enable = 1; - step_freq = 1; - } - -}*/ void looper_motor() { @@ -172,46 +107,35 @@ Step.period(1.0/step_freq); speed_old = speed; - if (step_freq < 800) { + if (step_freq < MIN_freq) { Enable = 1; } else { Enable = 0; } } - int main() { // Attach the HIDScope::send method from the scope object to the timer at 50Hz. Hier wordt de sample freq aangegeven. scopeTimer.attach_us(&scope, &HIDScope::send, 2e3); - /* Ticker log_timer; - //set up filters. Use external array for constants - arm_biquad_cascade_df1_init_f32(&lowpass_pot, 1 , lowpass_const, lowpass_pot_states); - log_timer.attach(looper_pot, 0.01);*/ - Ticker emgtimer; 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); emgtimer.attach(looper_emg, 0.002); Ticker looptimer; - looptimer.attach(looper_motor, 0.01); - + looptimer.attach(looper_motor, 0.01); //Uitzoeken waarom deze frequentie! + + //Microstepping control MS1 = 1; MS2 = 0; MS3 = 0; - //Step.period(1/step_freq); Step.write(0.5); // Duty cycle van 50% while (1) { 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("speed %.0f Hz p1 %.4f \n", step_freq, pot_value1_f32); //snelheid meting op lcd wait(0.01); - - } }