     }
   }
 
-  if(currentConveyorStatus == START_ROTATING_CONVEYOR){
-    conveyorStopTime = millis() + (long)currentConveyorCommand.duration*1000;   
-    driveConveyor();
-    currentConveyorStatus = ROTATING_CONVEYOR;
-  }
-
-  if(currentConveyorStatus == ROTATING_CONVEYOR){
-    if(millis() >= conveyorStopTime){
-      stopConveyor();
-    }
-  }
-
-  //Currently stopped, don't do anything.
-  // ASSUMES the robot is stopped when currentStatus is set to STOPPED.
-  if (currentStatus == STOPPED) {
-    // do nothing
-  }
-
-
   pubexcavatorStatus.publish(&excavatorStatus);// current rotation data for each wheel. 
 
   //Sync with ROS



//<---------------CODE TO SPIN BUCKET WHEEL---------------->
void rotateBucketClockwise(){
  for (int i = 0; i < 4; ++i)
  {
    driveClockwise(i, BUCKET_SPEED);
  }
}

void rotateBucketCounterclockwise(){
  for (int i = 0; i < 4; ++i)
  {
    driveCounterclockwise(i, BUCKET_SPEED);
  }
}


    return;
  } 
  
  //Currently stopped, don't do anything.
  // ASSUMES the robot is stopped when currentStatus is set to STOPPED.
  if (currentStatus == STOPPED) {
    // do nothing
  }

  
  if(currentConveyorStatus == START_ROTATING_CONVEYOR){
    conveyorStopTime = millis() + (long)conveyorRotationTime*1000;   
    driveConveyor();
    currentConveyorStatus = ROTATING_CONVEYOR;
  }

  if(currentConveyorStatus == ROTATING_CONVEYOR){
    if(millis() >= conveyorStopTime){
      stopConveyor();
