Diff: source/Main.cpp

Revision:

87:df8c869a5a52

Parent:

86:d8ea8a99fa3a

Parent:

81:a6f0f827dcb0

Child:

90:7f9d6e641a01

--- a/source/Main.cpp	Mon May 01 10:07:35 2017 +0000
+++ b/source/Main.cpp	Mon May 01 12:55:11 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();
@@ -116,6 +116,9 @@
             case 47:
                 state = move_in_search_for_brick();
                 break;
+            case 48:
+                state = generate_fake_target();
+                break;
 
 
             case 50:
@@ -145,8 +148,8 @@
                 state = 53;
                 wait_ms(1000);
                 break;
-                
-/******************************************************************************/
+
+                /******************************************************************************/
 //Testtools
 
             case 100:   //Testtool for IR sensors
@@ -164,19 +167,34 @@
                 printf("Color: %s\r\n",get_color()? str1 : str2);
                 wait_ms(500);
                 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;
+                break;
+
+            case 103:
+                move_for_distance(1.00f);
+                state = 104;
+                break;
+            case 104:
+                if(move_for_distance(0) < 0.0f) {
+                    stop_move();
+                    state = 0;
+                }
+                break;
         }
     }
 }
\ No newline at end of file