State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
Diff: main.cpp
- Revision:
- 42:cafa56da9ed2
- Parent:
- 41:8640b5782bc7
- Child:
- 44:0056118c01b2
diff -r 8640b5782bc7 -r cafa56da9ed2 main.cpp --- a/main.cpp Thu Nov 01 16:05:41 2018 +0000 +++ b/main.cpp Thu Nov 01 16:15:30 2018 +0000 @@ -230,7 +230,10 @@ debug_forward_kinematics = true; } - if (emg_1.get_is_envelope_over_threshold()) { + bool emg_1_state = emg_1.get_is_envelope_over_threshold(); + bool emg_2_state = emg_2.get_is_envelope_over_threshold(); + + if (emg_1_state) { led_green = 0; if (control_goes_up) { @@ -242,7 +245,7 @@ led_green = 1; } - if (emg_2.get_is_envelope_over_threshold()) { + if (emg_2_state) { led_blue = 0; if (control_goes_right) { speed_x = max_speed; @@ -316,12 +319,17 @@ main_motor.set_target_angle(main_target); sec_motor.set_target_angle(sec_target); + /* screen.get_screen_handle()->setTextCursor(0, 0); screen.get_screen_handle()->printf("M_a: %.6f \n", main_cur_angle); screen.get_screen_handle()->printf("S_a: %.6f \n", sec_cur_angle); screen.get_screen_handle()->printf("Vx: %.6f \n", main_target); screen.get_screen_handle()->printf("Vy: %.6f ", sec_target); screen.display(); + */ + + screen.display_emg_state(emg_1_state, emg_2_state); + screen.display(); if (ud_button.has_just_been_pressed()) { control_goes_up = !control_goes_up;