De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 30:bac3b60d6283
- Parent:
- 29:f51683a6cbbf
- Child:
- 31:b5188b6d45db
--- a/main.cpp Mon Oct 28 14:40:43 2019 +0000 +++ b/main.cpp Mon Oct 28 15:47:48 2019 +0000 @@ -227,11 +227,11 @@ if (calibrateNow == true) { // Only add values to EMG vectors if calibration flag is true emg1_cal.push_back(emg1_env); // Add values to calibration vector - emg1_cal_size = emg1_cal.size(); + // emg1_cal_size = emg1_cal.size(); // Used for debugging emg2_cal.push_back(emg2_env); // Add values to calibration vector - emg2_cal_size = emg1_cal.size(); + // emg2_cal_size = emg1_cal.size(); // Used for debugging emg3_cal.push_back(emg3_env); // Add values to calibration vector - emg3_cal_size = emg1_cal.size(); + // emg3_cal_size = emg1_cal.size(); // Used for debugging } } } @@ -301,7 +301,9 @@ { // Entry function if ( emg_state_changed == true ) { - emg_state_changed = false; + 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 } // Do nothing until end condition is met @@ -312,15 +314,21 @@ // 3. emg_operation (both calibrations have run) if ( button1_pressed ) { button1_pressed = false; // Disable button pressed function until next button press + button1.fall( NULL ); // Disable interrupt during calibration + button2.fall( NULL ); // Disable interrupt during calibration emg_curr_state = emg_cal_MVC; // Set next state emg_state_changed = true; // Enable entry functions } else if ( button2_pressed ) { button2_pressed = false; // Disable button pressed function until next button press + button1.fall( NULL ); // Disable interrupt during calibration + button2.fall( NULL ); // Disable interrupt during calibration emg_curr_state = emg_cal_rest; // Set next state emg_state_changed = true; // Enable entry functions } else if ( emg_MVC_cal_done && emg_rest_cal_done ) { + button1.fall( NULL ); // Disable interrupt during operation + button2.fall( NULL ); // Disable interrupt during operation emg_curr_state = emg_operation; // Set next state emg_state_changed = true; // Enable entry functions } @@ -372,6 +380,9 @@ 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 + + sampleNow = true; // Enable signal sampling in sampleSignal() + calibrateNow = false; // Disable calibration vector functionality in sampleSignal() } // Do stuff until end condition is met @@ -414,6 +425,8 @@ scope.set(2, emg2_out ); scope.set(3, emg3_out ); scope.send(); + + led_g = !led_g; // State transition guard @@ -484,8 +497,6 @@ led_r = 0; // LED on } emg_curr_state = emg_wait; // Start off in EMG Wait state - button1.fall( &button1Press ); // Change to state MVC calibration on button press - button2.fall( &button2Press ); // Change to state rest calibration on button press tickGlobal.attach( &tickGlobalFunc, Ts ); @@ -495,7 +506,8 @@ // Show that system is running // led_g = !led_g; - pc.printf("currentState: %i vectors size: %i %i %i\r\n", emg_curr_state, emg1_cal_size, emg2_cal_size, emg3_cal_size); - wait(1.0f); + // pc.printf("currentState: %i vectors size: %i %i %i\r\n", emg_curr_state, emg1_cal_size, emg2_cal_size, emg3_cal_size); + pc.printf("emg_state: %i emg1_env: %f emg1_out: %f emg1_th: %f emg1_factor: %f\r\n", emg_curr_state, emg1_env, emg1_out, emg1_th, emg1_factor); + wait(0.5f); } } \ No newline at end of file