State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
Diff: main.cpp
- Revision:
- 21:d541303a2ea6
- Parent:
- 20:af1a6cd7469b
- Child:
- 23:fb681b074a92
- Child:
- 35:38a5af0afee8
diff -r af1a6cd7469b -r d541303a2ea6 main.cpp --- a/main.cpp Wed Oct 31 18:22:39 2018 +0000 +++ b/main.cpp Wed Oct 31 18:54:07 2018 +0000 @@ -7,6 +7,7 @@ #include "motor.h" #include "motor_calibration.h" #include "forward_kinematics.h" +#include "inverse_kinematics.h" enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, failure}; // The possible states of the state machine @@ -170,6 +171,8 @@ void do_state_operation() { + static bool debug_forward_kinematics; + if(last_state != current_state) { last_state = current_state; // State just changed to this one. @@ -181,23 +184,48 @@ screen.display_up_down_arrow(control_goes_up); screen.display_left_right_arrow(control_goes_right); + + debug_forward_kinematics = true; } - // Using potmeters for debugging purposes; - double main_angle = ((potmeter1.read() * 2) - 1) * PI; - double sec_angle = ((potmeter2.read() * 2) - 1) * PI; - - double e_x = 0.0; - double e_y = 0.0; + if (debug_forward_kinematics) { + // Using potmeters for debugging purposes; + double main_angle = ((potmeter1.read() * 2) - 1) * PI; + double sec_angle = ((potmeter2.read() * 2) - 1) * PI; + + double e_x = 0.0; + double e_y = 0.0; + + forward_kinematics(main_angle, sec_angle, e_x, e_y); + + screen.get_screen_handle()->setTextCursor(0, 0); + screen.get_screen_handle()->printf("M_a: %.6f \n", main_angle); + screen.get_screen_handle()->printf("S_a: %.6f \n", sec_angle); + screen.get_screen_handle()->printf("X: %.6f \n", e_x); + screen.get_screen_handle()->printf("Y: %.6f ", e_y); + screen.display(); - forward_kinematics(main_angle, sec_angle, e_x, e_y); + } else { + // Using potmeters for debugging purposes; + double e_x = potmeter1.read(); + double e_y = potmeter2.read(); + + double main_angle = 0.0; + double sec_angle = 0.0; + + inverse_kinematics(e_x, e_y, main_angle, sec_angle); + + screen.get_screen_handle()->setTextCursor(0, 0); + screen.get_screen_handle()->printf("E_x: %.6f \n", e_x); + screen.get_screen_handle()->printf("E_y: %.6f \n", e_y); + screen.get_screen_handle()->printf("M_a: %.6f \n", main_angle); + screen.get_screen_handle()->printf("S_a: %.6f ", sec_angle); + screen.display(); + } - screen.get_screen_handle()->setTextCursor(0, 0); - screen.get_screen_handle()->printf("M_a: %.6f\n", main_angle); - screen.get_screen_handle()->printf("S_a: %.6f\n", sec_angle); - screen.get_screen_handle()->printf("X: %.6f\n", e_x); - screen.get_screen_handle()->printf("Y: %.6f", e_y); - screen.display(); + if (lr_button.has_just_been_pressed()) { + debug_forward_kinematics = !debug_forward_kinematics; + } /* double main_target = ((potmeter1.read() * 2) - 1) * PI;