 
 // State machine states
 const int STOPPED = 0;
-const int DRIVING = 1;
-const int DIGGING = 2;
-const int CONVEYING = 3;
+const int READY_TO_DRIVE = 1;
+const int DRIVING = 2;
+const int READY_TO_DIG = 3;
+const int DIGGING =  4;
+const int READY_TO_CONVEY = 5;
+const int CONVEYING = 6;
 
 int currentStatus = STOPPED;
 
 const int DRIVE_SPEED = 20;
 const int ROTATION_SPEED = 20;
 
+unsigned long driveUntilTime = 0;
 
 //
 // ROS Node Initialization

  delayMicroseconds(1000);
}

// Wheel unit test.
// Each wheel should rotate individually.
void unitTest()

#include <ros.h>
// Include a custom command message.
#include <command2ros/ExcavatorCommand.h>
#include <std_msgs/String.h>

void newExcavatorCommandCallback(const command2ros::ExcavatorCommand& newManualCommand);

// Device constants
const int NUM_TRANSPORT_MOTORS = 4;
