       Serial.println( "RAISE TOP FLAP" );
       servoDriver.writeServo1( SERVO1_UP );
       fsmState = STATE_MOVE_ITEM_OUT_PHASE_ONE;
-      //delay( 1000 );
       Serial.println( "->> MOVE ITEM OUT PHASE ONE" );
       break;
 
     case STATE_MOVE_ITEM_OUT_PHASE_ONE: 
-
-      //usDistance = ultraSensor.distanceCm();
-      usDistance = -1;
       if ( stopMicroSwitch == LOW && moveItemStepper.distanceToGo() == 0 ) {
-        moveItemStepper.move( -4800 );
-        moveItemStepperIsRunning = true;
+        moveItemStepper.move( moveItemOutSteps );
         fsmState = STATE_MOVE_ITEM_OUT_PHASE_ONE;
       } else if ( moveItemStepper.distanceToGo() != 0) {
         moveItemStepper.run();
         fsmState = STATE_MOVE_ITEM_OUT_PHASE_ONE;
-      /*} else if( usDistance < 10 && moveItemStepperIsRunning == true ) {
-        moveItemStepper.run();
-        fsmState = STATE_MOVE_ITEM_OUT_PHASE_ONE;*/
       } else {
-        /*moveItemStepper.stop();
-        moveItemStepperIsRunning = false;*/
         fsmState = STATE_MOVE_ITEM_OUT_PHASE_TWO;
       }
       break;

  Serial.begin( 9600 );

  // Initialize components
  baseShield.begin();  
  //infraredReceiver.begin();  
  ultraSensor.begin();

  //moveItemStepper.setSpeed( 12 );
  moveItemStepper.setMaxSpeed( 6400.0 );
  moveItemStepper.setAcceleration( 38100.0 );

  servoDriver.Servo1_begin();


/* Flapping servo */
int SERVO1_UP = 130;
int SERVO1_DOWN = 115;
int lowerTopFlapDelay = 400;

/* Move item stepper */
int portNo = PORT_3;
int pulseLength = 30;
float maxSpeed = 6400.0;
float acceleration = 38100.0;
int moveItemInSteps = 10000;
int pullBackFlapSteps = 750;
int moveItemOutSteps = -4800;

void setup() {
  // Initailize FSM
