State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
Diff: main.cpp
- Revision:
- 34:ae62ebf4d494
- Parent:
- 33:543debddb3a9
- Child:
- 39:f119ca6fc821
diff -r 543debddb3a9 -r ae62ebf4d494 main.cpp --- a/main.cpp Thu Nov 01 14:39:55 2018 +0000 +++ b/main.cpp Thu Nov 01 14:55:47 2018 +0000 @@ -13,7 +13,10 @@ enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, demo, failure}; // The possible states of the state machine -// Global variables + + +// Controll directions for the demo controller. +enum DebugControlDirection {debug_up, debug_down, debug_left, debug_right}; Motor main_motor(D6, D7, D13, D12); @@ -267,6 +270,11 @@ } void do_state_demo() { + static DebugControlDirection control_direction; + static const double max_speed = 0.01; + static double speed_x; + static double speed_y; + if(last_state != current_state) { last_state = current_state; // State just changed. @@ -277,31 +285,44 @@ screen.clear_display(); screen.display_state_name("Demo mode!"); + control_direction = debug_up; + + speed_x = 0; + speed_y = 0; + screen.display_up_down_arrow(control_goes_up); screen.display_left_right_arrow(control_goes_right); } if (lr_button.has_just_been_pressed()) { - control_goes_up = !control_goes_up; - control_goes_right = !control_goes_right; - screen.display_up_down_arrow(control_goes_up); - screen.display_left_right_arrow(control_goes_right); + switch (control_direction) { + case debug_up: + control_direction = debug_right; + speed_x = max_speed; + speed_y = 0; + break; + case debug_right: + control_direction = debug_down; + speed_x = 0; + speed_y = -max_speed; + break; + case debug_down: + control_direction = debug_left; + speed_x = -max_speed; + speed_y = 0; + break; + case debug_left: + control_direction = debug_up; + speed_x = 0; + speed_y = max_speed; + break; + } } - if (ud_button.has_just_been_pressed()) { + if (ud_button.is_pressed()) { led_blue = 0; - - double speed_x = 0.01; - double speed_y = 0.01; - - if (!control_goes_right) { - speed_x = -speed_x; - } - if (!control_goes_up) { - speed_y = -speed_y; - } - + double main_cur_angle = main_motor.get_current_angle(); double sec_cur_angle = sec_motor.get_current_angle();