State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
Diff: main.cpp
- Revision:
- 39:f119ca6fc821
- Parent:
- 34:ae62ebf4d494
- Child:
- 40:7e8d0632757c
diff -r fd89e1301734 -r f119ca6fc821 main.cpp --- a/main.cpp Thu Nov 01 15:09:10 2018 +0000 +++ b/main.cpp Thu Nov 01 15:41:34 2018 +0000 @@ -5,10 +5,12 @@ #include "Button.h" #include "Screen.h" #include "motor.h" +#include "EMGFilter.h" #include "motor_calibration.h" #include "forward_kinematics.h" #include "inverse_kinematics.h" #include "end_effector_control.h" +#include "EMG_calibration.h" enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, demo, failure}; // The possible states of the state machine @@ -26,6 +28,9 @@ AnalogIn potmeter1(A5); // Analoge input van potmeter 1 -> Motor 1 AnalogIn potmeter2(A4); // Analoge input van potmeter 2 -> Motor 2 +EMGFilter emg_1(A0); +EMGFilter emg_2(A1); + States current_state; // Defining the state we are currently in States last_state; // To detect state changes. @@ -165,29 +170,37 @@ void do_state_calib_bicep1() { + static EMG_calibration calibration = EMG_calibration(&screen, &emg_1); + if(last_state != current_state) { last_state = current_state; // State just changed to this one. screen.clear_display(); screen.display_state_name("EMG 1 calibration"); + + calibration.start(); } - if (ud_button.has_just_been_pressed()) { + if (ud_button.has_just_been_pressed() && calibration.is_calibrated()) { current_state = calib_bicep2; } } void do_state_calib_bicep2() { + static EMG_calibration calibration = EMG_calibration(&screen, &emg_2); + if(last_state != current_state) { last_state = current_state; // State just changed to this one. screen.clear_display(); screen.display_state_name("EMG 2 calibration"); + + calibration.start(); } - if (ud_button.has_just_been_pressed()) { - current_state = homing; + if (ud_button.has_just_been_pressed() && calibration.is_calibrated()) { + current_state = operation; } } @@ -444,6 +457,9 @@ // So that the waiting state knows it just got started. last_state = operation; + emg_1.start(emg_period); + emg_2.start(emg_period); + // Start the button polling ticker. button_ticker.attach(&poll_buttons, button_poll_interval);