De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 36:ec2bb2a02856
- Parent:
- 35:e82834e62e44
- Child:
- 37:806c7c8381a7
--- a/main.cpp Tue Oct 29 16:00:53 2019 +0000 +++ b/main.cpp Tue Oct 29 16:23:41 2019 +0000 @@ -55,6 +55,8 @@ double emg1_out; double emg1_out_prev; double emg1_dt; +double emg1_dt_prev; +double emg1_dtdt; double emg1_norm; vector<double> emg1_cal; int emg1_cal_size; @@ -429,6 +431,7 @@ emg3_norm = emg3_env * emg3_factor; // Idem emg1_out_prev = emg1_out; // Set previous emg_out signal + emg1_dt_prev = emg1_dt; // Set previous emg_out_dt signal // Set normalized EMG output signal (CAN BE MOVED TO EXTERNAL FUNCTION BECAUSE IT IS REPEATED 3 TIMES) if ( emg1_norm < emg1_th ) { // If below threshold, emg_out = 0 (ignored) emg1_out = 0.0; @@ -440,6 +443,7 @@ emg1_out = rescale(emg1_norm, 0, 1, emg1_th, 1); } emg1_dt = (emg1_out - emg1_out_prev) / Ts; // Calculate derivative of filtered normalized output signal + emg1_dtdt = (emg1_dt - emg1_dt_prev) / Ts; // Calculate acceleration of filtered normalized output signal emg1_out = emg1_out * emg1_dir; // Set direction of EMG output // Idem for emg2 @@ -463,9 +467,9 @@ // Set HIDScope outputs scope.set(0, emg1 ); - scope.set(1, emg1_env ); - scope.set(2, emg1_out ); - scope.set(3, emg1_dt ); + scope.set(1, emg1_out ); + scope.set(2, emg1_dt ); + scope.set(3, emg1_dtdt ); //scope.set(2, emg2_out ); //scope.set(3, emg3_out ); scope.send();