 #include "ExcavatorDriverROS.h"
 #include <command2ros/ExcavatorCommand.h>
 #include <std_msgs/String.h>
+#include <std_msgs/Int16.h>
 
 
 //<---------------- Set Up ------------------------------->

    }
  }

  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

  stopMovementMotors();

  //<------ Run Unittests Here ------>
  //unitTest();
  //unitTestDrive();
  //unitTestBucketWheelSpin();
  //unitTestBucketWheelActuate();
  //unitTestConveyor();
  //stopAllMotors();
}

