vf
Dependencies: Servo ServoArm mbed
Fork of PES_Official by
Diff: Sources/main.cpp
- Revision:
- 16:f157e5ccd7d3
- Parent:
- 15:915f8839fe48
diff -r 915f8839fe48 -r f157e5ccd7d3 Sources/main.cpp --- 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; } /* */