De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 35:e82834e62e44
- Parent:
- 34:13fac02ef324
- Child:
- 36:ec2bb2a02856
--- a/main.cpp Tue Oct 29 13:50:28 2019 +0000 +++ b/main.cpp Tue Oct 29 16:00:53 2019 +0000 @@ -15,7 +15,7 @@ */ // PC serial connection -HIDScope scope( 4 ); +HIDScope scope( 4 ); MODSERIAL pc(USBTX, USBRX); // LED @@ -53,9 +53,12 @@ double emg1_factor; double emg1_th; double emg1_out; +double emg1_out_prev; +double emg1_dt; double emg1_norm; vector<double> emg1_cal; int emg1_cal_size; +int emg1_dir = 1; double emg2; double emg2_env; @@ -67,6 +70,7 @@ double emg2_norm; vector<double> emg2_cal; int emg2_cal_size; +int emg2_dir = 1; double emg3; double emg3_env; @@ -78,6 +82,7 @@ double emg3_norm; vector<double> emg3_cal; int emg3_cal_size; +int emg3_dir = 1; // Initialize tickers and timeouts Ticker tickSample; @@ -133,11 +138,13 @@ // Return max value of vector double getMax(const vector<double> &vect) { - double curr_max = 0.0; + double curr_max = 0.0; int vect_n = vect.size(); - + for (int i = 0; i < vect_n; i++) { - if (vect[i] > curr_max) { curr_max = vect[i]; }; + if (vect[i] > curr_max) { + curr_max = vect[i]; + }; } return curr_max; } @@ -174,6 +181,24 @@ return output; } +// Check filter stability +bool checkBQChainStable() +{ + bool n_stable = bqc1_notch.stable(); // Check stability of all BQ Chains + bool hp_stable = bqc1_high.stable(); + bool l_stable = bqc1_low.stable(); + + if (n_stable && hp_stable && l_stable) { + return true; + } else { + return false; + } +} + +/* +------ BUTTON FUNCTIONS ------ +*/ + // Handle button press void button1Press() { @@ -186,17 +211,29 @@ button2_pressed = true; } -// Check filter stability -bool checkBQChainStable() +// Toggle EMG direction +void toggleEMG1Dir() { - bool n_stable = bqc1_notch.stable(); // Check stability of all BQ Chains - bool hp_stable = bqc1_high.stable(); - bool l_stable = bqc1_low.stable(); + switch( emg1_dir ) { + case -1: + emg1_dir = 1; + break; + case 1: + emg1_dir = -1; + break; + } +} - if (n_stable && hp_stable && l_stable) { - return true; - } else { - return false; +// Toggle EMG direction +void toggleEMG2Dir() +{ + switch( emg1_dir ) { + case -1: + emg1_dir = 1; + break; + case 1: + emg1_dir = -1; + break; } } @@ -291,7 +328,7 @@ // Entry function if ( emg_state_changed == true ) { emg_state_changed = false; // Disable entry functions - + button1.fall( &button1Press ); // Change to state MVC calibration on button1 press button2.fall( &button2Press ); // Change to state rest calibration on button2 press } @@ -376,6 +413,11 @@ emg2_th = emg2_rest * emg2_factor + margin_percentage/100; // Set normalized rest threshold emg3_factor = 1 / emg3_MVC; // Factor to normalize MVC emg3_th = emg3_rest * emg3_factor + margin_percentage/100; // Set normalized rest threshold + + + // ------- TO DO: MAKE SURE THESE BUTTONS DO NOT BOUNCE (e.g. with button1.rise() ) ------ + //button1.fall( &toggleEMG1Dir ); // Change to state MVC calibration on button1 press + //button2.fall( &toggleEMG2Dir ); // Change to state rest calibration on button2 press sampleNow = true; // Enable signal sampling in sampleSignal() calibrateNow = false; // Disable calibration vector functionality in sampleSignal() @@ -386,6 +428,7 @@ emg2_norm = emg2_env * emg2_factor; // Idem emg3_norm = emg3_env * emg3_factor; // Idem + emg1_out_prev = emg1_out; // Set previous emg_out 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; @@ -396,6 +439,8 @@ // Outputs are scaled to range [0, 1] 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_out = emg1_out * emg1_dir; // Set direction of EMG output // Idem for emg2 if ( emg2_norm < emg2_th ) { @@ -405,6 +450,7 @@ } else { emg2_out = rescale(emg2_norm, 0, 1, emg2_th, 1); } + emg2_out = emg2_out * emg2_dir; // Set direction of EMG output // Idem for emg3 if ( emg3_norm < emg3_th ) { @@ -418,8 +464,8 @@ // Set HIDScope outputs scope.set(0, emg1 ); scope.set(1, emg1_env ); - scope.set(2, emg1_norm ); - scope.set(3, emg1_out ); + scope.set(2, emg1_out ); + scope.set(3, emg1_dt ); //scope.set(2, emg2_out ); //scope.set(3, emg3_out ); scope.send(); @@ -494,7 +540,7 @@ } else { led_r = 0; // LED on } - + emg_curr_state = emg_wait; // Start off in EMG Wait state tickGlobal.attach( &tickGlobalFunc, Ts ); // Start global ticker