Diff: Sources/main.cpp

Revision:

16:f157e5ccd7d3

Parent:

15:915f8839fe48

--- a/Sources/main.cpp	Thu May 11 18:57:45 2017 +0000
+++ b/Sources/main.cpp	Mon May 15 17:35:24 2017 +0000
@@ -63,11 +63,12 @@
     //int lastFun = -1;                   //Is used, to check if the same action in Robot::search() was made multiple times.
     //int found = 0;                      //0:= no block available, 1 := a lego is ready to be picked up
     //int done = 0;
-
-    enum states { search = 0, LeisteDown, forward, downward, down, upward, color, backwardDrop, readyDrop, backward, LeisteUp };
+    int color;
+    
+    enum states { search = 0, LeisteDown, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };
 
     int state = search;
-    wait(5);
+    
     while( 1 ) {
         
         if ( timer > TIMEOUT ) {
@@ -76,28 +77,57 @@
         
         switch( state ) {
             case search:
-                if( 1 || sam.search(&timer) ){
-                  state = LeisteDown;
-                  timer = 0;
+                if( sam.search(&timer) ){
+                    //sam.Greifer.nullPos();
+                    state = LeisteDown;
+                    timer = 0;
                 }
-                
                 break;
        
             case LeisteDown:
-                if( sam.Leiste.UpToDown() ){
-                  sam.Greifer.leave();
-                  state = forward;
-                  timer = 0;
+                if( sam.Leiste.upToDown() ){
+                    //sam.Greifer.leave();
+                    state = push;
+                    timer = 0;
+                }
+                break;
+                
+            case push:{
+                static int i = 0;
+                if( i > 5 ){
+                    sam.stop();
+                    i = 0;
+                    state = backOff;
+                    timer = 0;
+                }
+                else{
+                    sam.driveSlowly();
+                    i++;
                 }
+                break;
+            }
+            
+            case backOff:{
+                static int i = 0;
+                if( i > 5 ){
+                    sam.stop();
+                    i = 0;
+                    state = forward;
+                    timer = 0;
+                }
+                else{
+                    sam.driveBackSlowly();
+                    i++;
+                }
+                break;
+            }
                 
-                break;
             
             case forward:
                 if( sam.Arm.backToCollect() ){
                     state = downward;
                     timer = 0;
                 }
-                
                 break;
 
             case downward:
@@ -105,7 +135,6 @@
                     state = down;
                     timer = 0;
                 }
-                
                 break;
 
             case down:
@@ -113,20 +142,18 @@
                     state = upward;
                     timer = 0;
                 }
-                
                 break;
             
             case upward:
                 if( sam.Arm.downToCollect() ){
-                    state = color;
+                    state = colorS;
                     timer = 0;
                 }
-                
                 break;
             
-            case color: {
+            case colorS:
                 led = 1;
-                int color = sam.FarbVoltage.read();
+                color = sam.FarbVoltage.read();
                 
                 if( color == -1 ){
                     //Do nothing
@@ -134,7 +161,7 @@
                 
                 
                 else if( color == 0 || color == GREEN ){
-                    state = backwardDrop;
+                    state = backward;
                     led = 0;
                     timer = 0;
                 }
@@ -149,19 +176,15 @@
                     //Shit...
                 }
                 break;
-            }
-            
-            case backwardDrop:
-                if( sam.Arm.collectToBack() ){
-                    state = readyDrop;
-                    timer = 0;
-                }
-                
-                break;
-                
+             
             case readyDrop:
                 if( sam.Greifer.leave() ){
-                    state = backward;
+                    if( color == GREEN || color == 0 ){
+                        state = LeisteUp;
+                    }
+                    else{
+                        state = backward;
+                    }
                     timer = 0;
                 }
                 
@@ -169,22 +192,32 @@
                 
             case backward:
                 if( sam.Arm.collectToBack() ){
-                    sam.Greifer.nullPos();
+                    //sam.Greifer.nullPos();
                     state = LeisteUp;
                     timer = 0;
+                    if( color == GREEN || color == 0 ){
+                        state = readyDrop;
+                        sam.Greifer.leave();
+                    }
+                    else{
+                        state = LeisteUp;
+                    }
                 }
-                
                 break;
                 
             case LeisteUp:
-                if( sam.Leiste.DownToUp() ){
+                if( sam.Leiste.downToUp() ){
                     state = search;
                     timer = 0;
-                    wait(2);
                 }
                 break;
         }
         
+    /*    
+            sam.leds[1] = sam.sensors[RIGHT] < NEAR;
+            sam.leds[5] = sam.sensors[LEFT] < NEAR;
+            sam.leds[4] = sam.sensors[FWD] < NEAR;
+      */  
         timer++;
         wait(0.1f);
     }
@@ -193,7 +226,6 @@
 }
         
         
-        
         /*
         if (sam.search(&counter, &timer)) {
 
@@ -229,20 +261,20 @@
 
     initializeDistanceSensors();
     sam.stop();
+    
+    //DigitalOut userLed(LD2);
 
     while ( 1 ){
-        for (int i=0; i<6; i++){
-            sam.sensors[i] < NEAR ? sam.leds[i] = 1 : sam.leds[i] = 0;
-        }
-
-        sam.turnLeftS();
+        sam.sensors[FWD_L] < NEAR ?       sam.leds[4] = 1         : sam.leds[4] = 0;
+        sam.sensors[RIGHT_L] < NEAR ?     sam.leds[RIGHT_L] = 1   : sam.leds[RIGHT_L] = 0;
+        sam.sensors[LEFT_L] < NEAR ?      sam.leds[LEFT_L] = 1    : sam.leds[LEFT_L] = 0;
+        //sam.sensors[FWD_L] < NEAR ?     sam.leds[2] = 1 : sam.leds[4] = 0;
+        //sam.sensors[RIGHT_L] < NEAR ?   sam.leds[3] = 1 : sam.leds[4] = 0;
+        //sam.sensors[LEFT_L] < NEAR ?    userLED     = 1 : sam.leds[4] = 0;
+        
+        wait (0.2f);
+    }
 
-        while (sam.sensors[FWD] < NEAR){
-            wait(0.05f);
-            sam.stop();
-        }
-
-    }
     return 0;
 }
 /* */