     case STATE_PULL_BACK_FLAP: 
       Serial.println( "PULL BACK FLAP" );
       if ( stopMicroSwitch == LOW ) {
-        moveItemStepper.step( -300 );
+        moveItemStepper.step( 300 );
         fsmState = STATE_LOWER_TOP_FLAP;
       } 
       break;
     
     case STATE_LOWER_TOP_FLAP:
       Serial.println( "LOWER TOP FLAP" );    
-      servoDriver.writeServo1( 50 );
+      servoDriver.writeServo1( SERVO1_DOWN );
       delay( 1000 );
       fsmState = STATE_RELEASE_FLAP; 
     break;  
     
     case STATE_RELEASE_FLAP: 
       Serial.println( "RELEASE FLAP" );
-      moveItemStepper.step( 300 );
+      moveItemStepper.step( -300 );
       fsmState = STATE_RAISE_TOP_FLAP;    
       break;
     
     case STATE_RAISE_TOP_FLAP: 
       Serial.println( "RAISE TOP FLAP" );
-      servoDriver.writeServo1( 90 );
-      fsmState = STATE_MOVE_ITEM_OUT;
+      servoDriver.writeServo1( SERVO1_UP );
+      fsmState = STATE_MOVE_ITEM_OUT_PHASE_ONE;
       break;
 
-    case STATE_MOVE_ITEM_OUT: 
-      Serial.println( "MOVE ITEM OUT" );
-      if ( usDistance < 6 ) {
-        moveItemStepper.step( 600 );
-        fsmState = STATE_MOVE_ITEM_OUT;
+    case STATE_MOVE_ITEM_OUT_PHASE_ONE: 
+      Serial.println( "MOVE ITEM OUT PHASE ONE" );
+      if ( usDistance > 10 ) {
+        moveItemStepper.step( -600 );
+        fsmState = STATE_MOVE_ITEM_OUT_PHASE_ONE;
+      } else {
+        fsmState = STATE_MOVE_ITEM_OUT_PHASE_TWO;
+      }
+      break;
+
+    case STATE_MOVE_ITEM_OUT_PHASE_TWO: 
+      Serial.println( "MOVE ITEM OUT PHASE TWO" );
+      if ( usDistance < 10 ) {
+        moveItemStepper.step( -600 );
+        fsmState = STATE_MOVE_ITEM_OUT_PHASE_TWO;
       } else {
         fsmState = STATE_WAIT_FOR_ITEM;
       }

      Serial.println( "stopMicroSwitch = ");
      Serial.println( stopMicroSwitch );
      delay( 200 );
      if ( usDistance < 6 && stopMicroSwitch == HIGH ) {
        moveItemStepper.step( -100 );
        fsmState = STATE_MOVE_ITEM_IN;
      }
      break;
      
    case STATE_MOVE_ITEM_IN:
      Serial.println( "MOVE ITEM IN" );
      Serial.println( "stopMicroSwitch = ");
      Serial.println( stopMicroSwitch );
      if ( usDistance < 6 && stopMicroSwitch == HIGH ) {
        moveItemStepper.step( -100 );
        fsmState = STATE_MOVE_ITEM_IN;
      } else if ( stopMicroSwitch == LOW ) {
        fsmState = STATE_PULL_BACK_FLAP;

Me_InfraredReceiver infraredReceiver; /* Note: As the code need to use interrupt 0, it works only when the Me - Infrared Receiver module is connected to the port 4(PORT_4) of Me - Base Shield. */
Me_UltrasonicSensor ultraSensor( PORT_5 ); //Ultrasonic module can ONLY be connected to port 3, 4, 5, 6, 7, 8 of base shield. ultraSensor.distanceCm()
MakeblockStepper moveItemStepper( 3200, baseShield, PORT_3 );
Me_ServoDriver servoDriver( PORT_2 ); //can ONLY be PORT_1,PORT_2

int SERVO1_UP = 130;
int SERVO1_DOWN = 115;

void setup() {
  // Initailize FSM
