Diff: source/Main.cpp

Revision:

80:956f65714207

Parent:

79:92b9d083322d

Child:

81:a6f0f827dcb0

Child:

86:d8ea8a99fa3a

diff -r 92b9d083322d -r 956f65714207 source/Main.cpp
--- a/source/Main.cpp	Wed Apr 26 07:30:03 2017 +0000
+++ b/source/Main.cpp	Mon May 01 07:39:18 2017 +0000
@@ -67,6 +67,7 @@
 
             case 15:
                 state = initialisation();   //init servo is included
+                //Enter Testtool - State here
                 state = 100;
                 break;
             case 16:
@@ -151,7 +152,11 @@
                 state = 53;
                 wait_ms(1000);
                 break;
-            case 100:
+                
+/******************************************************************************/
+//Testtools
+
+            case 100:   //Testtool for IR sensors
                 printf("sensor 0: %f \r\n", getDistanceIR(0));
                 printf("sensor 1: %f \r\n", getDistanceIR(1));
                 printf("sensor 2: %f \r\n", getDistanceIR(2));
@@ -160,6 +165,25 @@
                 printf("sensor 5: %f \r\n\n", getDistanceIR(5));
                 state = 100;
                 break;
+            case 101:   //Testtool for get_color
+                char str1[] = "GREEN";
+                char str2[] = "RED";
+                printf("Color: %s\r\n",get_color()? str1 : str2);
+                wait_ms(500);
+                state = 101;
+                
+            case 102:   //Testtool for grabbing functions
+                open_grabber();
+                wait_ms(500);
+                arm_position_grabbing();
+                wait_ms(500);
+                //move_forward_for_distance(5.0);
+                wait_ms(500);
+                close_grabber();
+                wait_ms(500);
+                arm_position_release();
+                wait_ms(500);
+                state = 102;
         }
     }
 }
\ No newline at end of file