State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
Diff: main.cpp
- Revision:
- 23:fb681b074a92
- Parent:
- 21:d541303a2ea6
- Child:
- 24:e1092f95c82b
diff -r 720a410c4980 -r fb681b074a92 main.cpp --- a/main.cpp Thu Nov 01 06:55:10 2018 +0000 +++ b/main.cpp Thu Nov 01 08:20:36 2018 +0000 @@ -151,8 +151,11 @@ void do_state_homing() { - double main_home = PI * 0.5; - double sec_home = 0; + const double home_x = 0.6524; // Meters. + const double home_y = 0.3409; + + double main_home; + double sec_home; if(last_state != current_state) { last_state = current_state; @@ -160,6 +163,8 @@ screen.clear_display(); screen.display_state_name("Homing"); + inverse_kinematics(home_x, home_y, main_home, sec_home); + main_motor.set_target_angle(main_home); sec_motor.set_target_angle(sec_home); }