Diff: source/Main.cpp

Revision:

81:a6f0f827dcb0

Parent:

80:956f65714207

Child:

87:df8c869a5a52

diff -r 956f65714207 -r a6f0f827dcb0 source/Main.cpp
--- a/source/Main.cpp	Mon May 01 07:39:18 2017 +0000
+++ b/source/Main.cpp	Mon May 01 09:22:29 2017 +0000
@@ -1,7 +1,7 @@
 #include "Main.h"
 
 
-int state = 15;
+int state = 10;
 bool positioning_state = 0;
 bool mapping_state = 0;
 
@@ -40,6 +40,7 @@
         //State and Transition Table can be found in the State_Machine.xlsx
         //*******************************************************************************
         //printf("state: %d\r\n",state);
+        printf("State - %d\r\n",state);
         switch (state) {
             case 0:
                 state = emergency_shutdown();
@@ -67,11 +68,10 @@
 
             case 15:
                 state = initialisation();   //init servo is included
-                //Enter Testtool - State here
-                state = 100;
                 break;
             case 16:
                 state = inital_arm_positioning();
+                state = 11;
                 break;
             case 17:
                 state = initial_positioning();
@@ -173,15 +173,18 @@
                 state = 101;
                 
             case 102:   //Testtool for grabbing functions
+                arm_position_release();
+                wait_ms(500);
                 open_grabber();
                 wait_ms(500);
                 arm_position_grabbing();
-                wait_ms(500);
+                wait_ms(1000);
                 //move_forward_for_distance(5.0);
-                wait_ms(500);
                 close_grabber();
                 wait_ms(500);
-                arm_position_release();
+                if (get_color() == 0) {
+                    open_grabber();
+                }
                 wait_ms(500);
                 state = 102;
         }