De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 39:f9042483b921
- Parent:
- 38:8b597ab8344f
- Child:
- 40:c6dffb676350
--- a/main.cpp Wed Oct 30 15:49:09 2019 +0000 +++ b/main.cpp Wed Oct 30 16:33:01 2019 +0000 @@ -314,14 +314,14 @@ // Extract EMG scale data from calibration switch( emg_curr_state ) { - case emg_cal_MVC: + case emg_cal_MVC: // In case of MVC calibration emg1_MVC = getMax(emg1_cal); // Store max value of MVC globally emg2_MVC = getMax(emg2_cal); emg3_MVC = getMax(emg3_cal); emg_MVC_cal_done = true; // Set up transition to EMG operation mode break; - case emg_cal_rest: + case emg_cal_rest: // In case of rest calibration emg1_rest = getMean(emg1_cal); // Store mean of EMG in rest globally emg2_rest = getMean(emg2_cal); emg3_rest = getMean(emg3_cal); @@ -343,8 +343,9 @@ // Entry function if ( emg_state_changed == true ) { emg_state_changed = false; // Disable entry functions + + // Compute scale factors for all EMG signals double margin_percentage = 5; // Set up % margin for rest - emg1_factor = 1 / emg1_MVC; // Factor to normalize MVC emg1_th = emg1_rest * emg1_factor + margin_percentage/100; // Set normalized rest threshold emg2_factor = 1 / emg2_MVC; // Factor to normalize MVC @@ -357,64 +358,13 @@ //button1.fall( &toggleEMG1Dir ); // Change to state MVC calibration on button1 press //button2.fall( &toggleEMG2Dir ); // Change to state rest calibration on button2 press - emg_sampleNow = true; // Enable signal sampling in sampleSignals() - emg_calibrateNow = false; // Disable calibration vector functionality in sampleSignals() - } - - // Do stuff until end condition is met - emg1_norm = emg1_env * emg1_factor; // Normalize EMG signal with calibrated factor - emg2_norm = emg2_env * emg2_factor; // Idem - 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; - } else if ( emg1_norm > 1.0f ) { // If above MVC (e.g. due to filtering), emg_out = 1 (max value) - emg1_out = 1.0; - } else { // If in between threshold and MVC, scale EMG signal accordingly - // Inputs may be in range [emg_th, 1] - // Outputs are scaled to range [0, 1] - emg1_out = rescale(emg1_norm, 0, 1, emg1_th, 1); + emg_cal_done = true; // Let the global substate machine know that EMG calibration is finished } - 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 - if ( emg2_norm < emg2_th ) { - emg2_out = 0.0; - } else if ( emg2_norm > 1.0f ) { - emg2_out = 1.0; - } 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 ) { - emg3_out = 0.0; - } else if ( emg3_norm > 1.0f ) { - emg3_out = 1.0; - } else { - emg3_out = rescale(emg3_norm, 0, 1, emg3_th, 1); - } - - // Set HIDScope outputs - scope.set(0, emg1 ); - 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(); - - led_g = !led_g; + + // This state only runs its entry functions ONCE and then exits the EMG substate machine // State transition guard - if ( false ) { + if ( false ) { // EMG substate machine is terminated after running this state once, so there is no transition to next EMG substate emg_curr_state = emg_wait; // Set next state emg_state_changed = true; // Enable entry function } @@ -529,6 +479,9 @@ button2_pressed = false; global_curr_state = global_motor_cal; global_state_changed = true; + } else if ( emg_cal_done && motor_cal_done ) { // OPERATION MODE + global_curr_state = global_operation; + global_state_changed = true; } } @@ -540,17 +493,11 @@ global_state_changed = false; } - // Do stuff until end condition is met - doStuff(); + // Run EMG state machine until emg_cal_done flag is true + emg_state_machine(); // State transition guard - if ( motor_cal_done == true ) { // OPERATION MODE - emg_cal_done = true; - global_curr_state = global_operation; - global_state_changed = true; - } else if ( button1_pressed == true ) { // WAIT MODE - button1_pressed = false; - emg_cal_done = true; + if ( emg_cal_done == true ) { // WAIT MODE global_curr_state = global_wait; global_state_changed = true; } @@ -586,10 +533,62 @@ // Entry function if ( global_state_changed == true ) { global_state_changed = false; + + emg_sampleNow = true; // Enable signal sampling in sampleSignals() + emg_calibrateNow = false; // Disable calibration functionality in sampleSignals() } // Do stuff until end condition is met - doStuff(); + emg1_norm = emg1_env * emg1_factor; // Normalize current EMG signal with calibrated factor + emg2_norm = emg2_env * emg2_factor; // Idem + 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; + } else if ( emg1_norm > 1.0f ) { // If above MVC (e.g. due to filtering), emg_out = 1 (max value) + emg1_out = 1.0; + } else { // If in between threshold and MVC, scale EMG signal accordingly + // Inputs may be in range [emg_th, 1] + // 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_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 + if ( emg2_norm < emg2_th ) { + emg2_out = 0.0; + } else if ( emg2_norm > 1.0f ) { + emg2_out = 1.0; + } 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 ) { + emg3_out = 0.0; + } else if ( emg3_norm > 1.0f ) { + emg3_out = 1.0; + } else { + emg3_out = rescale(emg3_norm, 0, 1, emg3_th, 1); + } + + // Set HIDScope outputs + scope.set(0, emg1 ); + 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(); + + led_g = !led_g; // State transition guard if ( false ) { // Always stay in operation mode (can be changed) @@ -611,7 +610,6 @@ break; case global_emg_cal: do_global_emg_cal(); - emg_state_machine(); break; case global_motor_cal: do_global_motor_cal(); @@ -636,7 +634,6 @@ emg2 = emg2_in.read(); emg3 = emg3_in.read(); - double emg1_n = bqc1_notch.step( emg1 ); // Filter notch double emg1_hp = bqc1_high.step( emg1_n ); // Filter highpass double emg1_rectify = fabs( emg1_hp ); // Rectify @@ -677,7 +674,7 @@ /* ------------------------------ MAIN FUNCTION ------------------------------ */ -void main() +int main() { pc.baud(115200); // MODSERIAL rate pc.printf("Starting\r\n");