 {
  //Currently stopped, don't do anything.
   // ASSUMES the robot is stopped when currentStatus is set to STOPPED.
-  if (currentStatus == STOPPED) {
-    // do nothing
-  }
-  else if (currentStatus == DRIVING)
-  {
-   // TODO: Drive 
-  }
-  else if(currentStatus == DIGGING)
-  {
-    // TODO: Dig 
-  }
-  else if(currentStatus == CONVEYING)
-  {
-    // TODO: Convey 
-  }
+switch(currentStatus)
+{
+ case READY_TO_DRIVE:
+   driveTires();
+   driveUntilTime = millis() + (long)lastExcavatorCommand.drive_duration;
+ break;
+ case DRIVING:
+   if(millis() > driveUntilTime)
+   {
+     stopAllMotors();
+     // TODO: Change this to excavating, if necessary.
+    currentStatus = STOPPED;
+   }
+ break;
+  
+ case STOPPED:
+  // fall through
+ default:
+ // TODO: Convey / Dig 
+ break;
+}
+  
   //TODO: Publish sensor / state data?
   //Sync with ROS
   excavatorNode.spinOnce(); // Check for subscriber update/update timestamp

  delayMicroseconds(1000);
}

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


// State machine states
const int STOPPED = 0;
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
