 
   stopAllMotors(false);
   //unitTest();
+  unitTestConveyor();
+  unitTestWinch();
 
   // Initialize the current wheel status message
   setupWheelStatus();

void unitTestConveyor()
{
  // Test the conveyor:
  // 2s "dumping" / forwards (clockwise), 2s "collecting" / backwards (counterclockwise).

  driveClockwise(LEFT_CONVEYOR_MOTOR_ID, CONVEYOR_SPEED);
  driveClockwise(RIGHT_CONVEYOR_MOTOR_ID, CONVEYOR_SPEED);
  delaySeconds(2);
  driveCounterclockwise(LEFT_CONVEYOR_MOTOR_ID, CONVEYOR_SPEED);
  driveCounterclockwise(RIGHT_CONVEYOR_MOTOR_ID, CONVEYOR_SPEED);
  delaySeconds(2);
  driveCounterclockwise(LEFT_CONVEYOR_MOTOR_ID, 0);
  driveCounterclockwise(RIGHT_CONVEYOR_MOTOR_ID, 0);
  delaySeconds(2);
  
}



//Conveyor and winch system constants
const char CONVEYOR_SPEED = 20;
const char WINCH_SPEED = 15;

int conveyorRotationTime = 0;
long conveyorStopTime = 0;
