Diff: Sources/main.cpp

Revision:

15:915f8839fe48

Parent:

14:b61fbd13a7c9

Child:

16:f157e5ccd7d3

Child:

17:4e1be70bdedb

--- a/Sources/main.cpp	Wed May 10 19:53:08 2017 +0000
+++ b/Sources/main.cpp	Thu May 11 18:57:45 2017 +0000
@@ -36,13 +36,13 @@
 Servo servoGreifer(PC_7);
 
 //Leiste:
-Servo servoLeiste(PB_7);
+Servo servoLeiste(PB_6);
 
 //Farbsensor:
 AnalogIn FarbVoltage(A0);
 DigitalOut led(D2);
 
-Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer ); //Implement the Farbsensor into the Robot init function!!
+Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste ); //Implement the Farbsensor into the Robot init function!!
 
 void initializeDistanceSensors()
 {
@@ -64,10 +64,10 @@
     //int found = 0;                      //0:= no block available, 1 := a lego is ready to be picked up
     //int done = 0;
 
-    enum states { search = 0, forward, downward, down, upward, color, backwardDrop, readyDrop, backward };
+    enum states { search = 0, LeisteDown, forward, downward, down, upward, color, backwardDrop, readyDrop, backward, LeisteUp };
 
     int state = search;
-
+    wait(5);
     while( 1 ) {
         
         if ( timer > TIMEOUT ) {
@@ -77,6 +77,15 @@
         switch( state ) {
             case search:
                 if( 1 || sam.search(&timer) ){
+                  state = LeisteDown;
+                  timer = 0;
+                }
+                
+                break;
+       
+            case LeisteDown:
+                if( sam.Leiste.UpToDown() ){
+                  sam.Greifer.leave();
                   state = forward;
                   timer = 0;
                 }
@@ -160,11 +169,20 @@
                 
             case backward:
                 if( sam.Arm.collectToBack() ){
-                    state = search;
+                    sam.Greifer.nullPos();
+                    state = LeisteUp;
                     timer = 0;
                 }
                 
                 break;
+                
+            case LeisteUp:
+                if( sam.Leiste.DownToUp() ){
+                    state = search;
+                    timer = 0;
+                    wait(2);
+                }
+                break;
         }
         
         timer++;