Diff: Sources/main.cpp

Revision:

18:a158713a0049

Parent:

17:4e1be70bdedb

--- a/Sources/main.cpp	Thu May 18 14:27:09 2017 +0000
+++ b/Sources/main.cpp	Fri May 19 06:40:33 2017 +0000
@@ -73,18 +73,13 @@
     //int done = 0;
     int color;
     
-    enum states { search = 0, LeisteDown, turn, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };
+    enum states { search = 0, LeisteDown, turn, push, pos, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };
 
     int state = search;
     
-                static float messung = 0;
+               // static float messung = 0;
                 
-    while( 1 ){
-        printf("\n\r%f", sam.see(FWD_L));
-        
-        wait(1.0f);
-            
-    }
+   
     
     while( 1 ) {
         
@@ -96,7 +91,7 @@
         //printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read());
         
         
-        //printf("\n\rcurrent main state: %d", state);
+        printf("\n\rcurrent main state: %d", state);
         
         sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0;
         switch( state ) {
@@ -118,8 +113,8 @@
                 break;
             
             case turn:
-                static int i = 0;
-                if( i > 7 ){
+               /* static int i = 0;
+                if( i > 4 ){
                     sam.stop();
                     state = push;
                     i = 0;
@@ -127,36 +122,54 @@
                 else{
                     i++;
                     sam.turnRight();
-                }
+                }*/
+                state = push;
                 break;
                 
             case push:{
                 static int i = 0;
-                if( i > 5 ){
+                if( i > 6 ){
+                    sam.stop();
+                    i = 0;
+                    state = pos;
+                    timer = 0;
+                }
+                else{
+                    sam.drive();
+                    i++;
+                }
+                break;
+                }
+                
+            case pos:{
+                static int i = 0;
+                if( i > 4 ){
                     sam.stop();
                     i = 0;
                     state = backOff;
                     timer = 0;
                 }
                 else{
+                    sam.turnRightS();
+                    i++;
+                }
+            
+                break;
+            }
+            
+            case backOff:{
+                static int i = 0;
+                if( i > 2 ){
+                    sam.stop();
+                    i = 0;
+                    state = forward;
+                    timer = 0;
+                }
+                else{
                     sam.driveSlowly();
                     i++;
                 }
-                break;
-            }
-            
-            case backOff:{
-                static int i = 0;
-                if( i > 1 ){
-                    sam.stop();
-                    i = 0;
-                    state = forward;
-                    timer = 0;
-                }
-                else{
-                    sam.driveBackSlowly();
-                    i++;
-                }
+                //state = forward;
                 break;
             }