       Serial.print( "distanceToGo = "); Serial.println( moveItemStepper.distanceToGo() );
       Serial.print( "speed = "); Serial.println( moveItemStepper.speed() );*/
       if ( stopMicroSwitch == HIGH && moveItemStepper.distanceToGo() == 0 ) {
-        moveItemStepper.move( moveItemInSteps );
+        moveItemStepper.setMaxSpeed( MOVE_ITEM_IN_MAX_SPEED );
+        moveItemStepper.move( MOVE_ITEM_IN_STEPS );
         Serial.print( "targetPosition = "); Serial.println( moveItemStepper.targetPosition() );
         fsmState = STATE_MOVE_ITEM_IN;
       } else if ( stopMicroSwitch == HIGH && moveItemStepper.distanceToGo() > 0 ) {


int fsmState;

Me_BaseShield baseShield;
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
AccelStepper moveItemStepper( forwardstep, backwardstep );
boolean moveItemStepperIsRunning = false;

/* 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

      break;

    case STATE_MOVE_ITEM_OUT_PHASE_ONE: 
      if (stopMicroSwitch == LOW && !moveItemStepperIsRunning && moveItemStepper.distanceToGo() == 0 ) {
        servoDriver.writeServo2( CONTACT_ON_POSITION );
        delay( 100 ); // In order to not putting to much pressure from the side on the solenoids
        moveItemStepper.setMaxSpeed( MOVE_ITEM_OUT_STEPPER_MAX_SPEED );
        moveItemStepper.move( MOVE_ITEM_OUT_STEPS );
        moveItemStepperIsRunning = true;
        baseShieldMotorDriver.runMotor1( OUTPUT_CONVEYANCE_SPEED );
        fsmState = STATE_MOVE_ITEM_OUT_PHASE_ONE;
      } else if ( moveItemStepper.distanceToGo() != 0) {
        moveItemStepper.run();
        fsmState = STATE_MOVE_ITEM_OUT_PHASE_ONE;
      } else {
        moveItemStepperIsRunning = false;
        fsmState = STATE_MOVE_ITEM_OUT_PHASE_TWO;
      }
      break;

    case STATE_MOVE_ITEM_OUT_PHASE_TWO: 
      Serial.println( "MOVE ITEM OUT PHASE TWO" );
        delay( OUTPUT_CONVEYANCE_DURATION );
        baseShieldMotorDriver.stopMotor1();
        servoDriver.writeServo2( CONTACT_OFF_POSITION );
        
      //if ( usDistance > 10 ) {
        //moveItemStepper.step( -600 );
        //moveItemStepper.move( -600 );
