Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

Diff: main.cpp

Revision:
23:fb681b074a92
Parent:
21:d541303a2ea6
Child:
24:e1092f95c82b
--- 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);
     }