Werkend met ledjes
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 19:a37cae6964ca
- Parent:
- 18:dddc8d9f7638
- Child:
- 20:4424d447f3cd
--- a/main.cpp Tue Oct 15 09:14:37 2019 +0000 +++ b/main.cpp Thu Oct 17 11:01:37 2019 +0000 @@ -9,10 +9,9 @@ Gravity compensation PID values General program layout + better names for EMG input */ - - #include "mbed.h" #include "MODSERIAL.h" #include "FastPWM.h" @@ -64,27 +63,37 @@ float I; float D; float I_counter; -} PID1 PID2; +}; +PID PID1; +PID PID2; float dt = 0.001; -int enc1_zero; //the zero position of the encoders, to be determined from the -int enc2_zero; //encoder calibration +float theta; +float L; +int enc1_zero = 0; //the zero position of the encoders, to be determined from the +int enc2_zero = 0; //encoder calibration int EMG1_filtered; int EMG2_filtered; int enc1_value; int enc2_value; float error1 = 0.0; float error2 = 0.0; +float last_error1 = 0.0; +float last_error2 = 0.0; +float action1; +float action2; bool state_changed = false; //used to see if the state is "starting" volatile bool but1_pressed = false; volatile bool but2_pressed = false; volatile bool failure_occurred = false; float pot_1; //used to keep track of the potentiometer values float pot_2; -bool enc_has_been_calibrated; bool EMG_has_been_calibrated; bool button1_pressed; bool button2_pressed; +const int EMG_cali_amount = 1000; +float past_EMG_values[EMG_cali_amount]; +int past_EMG_count = 0; void do_nothing() @@ -120,12 +129,29 @@ pc.printf("Started EMG calibration\r\n"); state_changed = false; } + if (past_EMG_count < EMG_cali_amount) { + past_EMG_values[past_EMG_count] = EMG1_filtered; + past_EMG_count++; + } + else { //calibration is has concluded + float sum = 0.0; + for(int i = 0; i<EMG_cali_amount; i++) { + sum += past_EMG_values[i]; + } + float mean = sum/(float)EMG_cali_amount; + EMG_values.min = mean; + //calibration done, moving to cali_enc + pc.printf("Calibration of the EMG is done, lower bound = %f", mean); + EMG_has_been_calibrated = true; + state_changed = true; + state = s_cali_enc; + } } void cali_enc() /* Calibration of the encoder. The encoder should be moved to the lowest - position for the linear stage and the most upright postition for the + position for the linear stage and the horizontal postition for the rotating stage. */ { @@ -134,9 +160,9 @@ state_changed = false; } if (button1_pressed) { + pc.printf("Encoder has been calibrated"); enc1_zero = enc1_value; enc2_zero = enc2_value; - enc_has_been_calibrated = true; button1_pressed = false; state = s_moving_magnet_off; state_changed = true; @@ -182,16 +208,29 @@ void measure_signals() { - float EMG1_raw; - float EMG2_raw; + //only one emg input, reference and plus + float EMG1_raw = EMG1.read(); + float EMG2_raw = EMG2.read(); + float filter_value1 = fabs(bq2.step(fabs(bq1.step(EMG1_raw - EMG2_raw)))); + + if (filter_value1 > EMG_values.max) { + EMG_values.max = filter_value1; + } + if (EMG_has_been_calibrated) { + EMG1_filtered = (filter_value1-EMG_values.min)/(EMG_values.max-EMG_values.min); + } + else { + EMG1_filtered = filter_value1; + } + enc1_value = enc1.getPulses(); enc2_value = enc2.getPulses(); - if (enc_has_been_calibrated) { - enc1_value -= enc1_zero; - enc2_value -= enc2_zero; - } - EMG1_raw = EMG1.read(); - EMG2_raw = EMG2.read(); + enc1_value -= enc1_zero; + enc2_value -= enc2_zero; + theta = (float)(enc1_value)/(float)(8400*2*PI); + L = (float)(enc2_value)/(float)(8400*2*PI); + + } void sample()