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

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


// Device constants
const int NUM_TRANSPORT_MOTORS = 4;

  delayMicroseconds(1000);
}

// Function that takes a SIGNED speed value and converts it to an UNSIGNED drive command.
void helpDrive(char motorID, char speed)
{
  if(speed < 0)
  {
    driveBackwards(motorID, -speed); 
  }
  else
  {
    driveForwards(motorID, speed);
  }
}

// Function to drive the wheels
void driveTires()
{
 if(currentStatus == READY_TO_DRIVE)
 {
   helpDrive(FRONT_LEFT_DRIVE_MOTOR_ID, lastExcavatorCommand.fl_drive_speed);
   helpDrive(FRONT_RIGHT_DRIVE_MOTOR_ID, lastExcavatorCommand.fr_drive_speed);
   helpDrive(BACK_LEFT_DRIVE_MOTOR_ID, lastExcavatorCommand.bl_drive_speed);
   helpDrive(BACK_RIGHT_DRIVE_MOTOR_ID, lastExcavatorCommand.br_drive_speed);
   currentStatus = DRIVING;
 } 
  
}

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