State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
Diff: main.cpp
- Revision:
- 32:b63b5837bcb1
- Parent:
- 30:a45bbfa6bd22
- Child:
- 33:543debddb3a9
diff -r e5b3b01d128a -r b63b5837bcb1 main.cpp --- a/main.cpp Thu Nov 01 13:29:49 2018 +0000 +++ b/main.cpp Thu Nov 01 14:11:30 2018 +0000 @@ -8,9 +8,10 @@ #include "motor_calibration.h" #include "forward_kinematics.h" #include "inverse_kinematics.h" +#include "end_effector_control.h" -enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, failure}; // The possible states of the state machine +enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, demo, failure}; // The possible states of the state machine // Global variables @@ -69,13 +70,6 @@ if (ud_button.has_just_been_pressed()) { current_state = calib_motor; } - - // TODO: - // THIS OPTION IS ONLY HERE FOR DEBUGGING PURPOSES. - // REMOVE WHEN THE DEMO STATE IS IMPLEMENTED. - if (lr_button.has_just_been_pressed()) { - current_state = operation; - } } void do_state_calib_motor() @@ -156,6 +150,9 @@ if (ud_button.has_just_been_pressed()) { current_state = calib_bicep1; } + if (lr_button.has_just_been_pressed()) { + current_state = demo; + } } void do_state_calib_bicep1() @@ -264,6 +261,62 @@ } } +void do_state_demo() { + if(last_state != current_state) { + last_state = current_state; + // State just changed. + // Update the display. + led_red = 1; + led_green = 0; + led_blue = 1; + screen.clear_display(); + screen.display_state_name("Demo mode!"); + + 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); + } + + if (ud_button.has_just_been_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(); + + double main_target, sec_target; + + end_effector_control(speed_x, speed_y, main_cur_angle, sec_cur_angle, main_target, sec_target); + + 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(); + } +} + void do_state_failure() { if(last_state != current_state) { @@ -308,6 +361,9 @@ case operation: do_state_operation(); break; + case demo: + do_state_demo(); + break; case failure: do_state_failure(); break;