   EncoderMR::setupEncoderMR();
   EncoderRR::setupEncoderRR();
 
+  //Initialize the ROS Node
   sabertoothDriverNode.initNode();
-  sabertoothDriverNode.subscribe(mcSUB);
-
-  // Communicate with the Sabertooth
+  sabertoothDriverNode.subscribe(commandSubscriber);
+  sabertoothDriverNode.advertise(pubwheelStatus);
+  sabertoothDriverNode.advertise(pubDebug);
+
+  // Open communication with Saberteeth
   Serial3.begin(9600);
 
   // Initialize the message
-  currentRotationMessage.fl_articulation_angle = 0;
-  currentRotationMessage.fr_articulation_angle = 180;
-  currentRotationMessage.ml_articulation_angle = 0;
-  currentRotationMessage.mr_articulation_angle = 180;
-  currentRotationMessage.rl_articulation_angle = 0;
-  currentRotationMessage.rr_articulation_angle = 180;
-  
-  currentRotationMessage.fl_drive_speed = 0;
-  currentRotationMessage.fr_drive_speed = 0;
-  currentRotationMessage.ml_drive_speed = 0;
-  currentRotationMessage.mr_drive_speed = 0;
-  currentRotationMessage.rl_drive_speed = 0;
-  currentRotationMessage.rr_drive_speed = 0;
-
-  // Initialize current wheel status: Assume we're in "closed" position
-  currentWheelStatus[FRONT_LEFT_DRIVE_MOTOR_ID].orientation = 0.0;     //.fl_articulation_angle = 0.0;
-  currentWheelStatus[FRONT_RIGHT_DRIVE_MOTOR_ID].orientation = 180.0;   //.fr_articulation_angle = 180.0;
-  currentWheelStatus[MIDDLE_LEFT_DRIVE_MOTOR_ID].orientation = 0.0;//.ml_articulation_angle = 0.0;
-  currentWheelStatus[MIDDLE_RIGHT_DRIVE_MOTOR_ID].orientation = 180.0;//.mr_articulation_angle = 180.0;
-  currentWheelStatus[REAR_LEFT_DRIVE_MOTOR_ID].orientation = 0.0;//.rl_articulation_angle = 0.0;
-  currentWheelStatus[REAR_RIGHT_DRIVE_MOTOR_ID].orientation = 180.0; //.rr_articulation_angle = 180.0;
-  
-  currentWheelStatus[FRONT_LEFT_DRIVE_MOTOR_ID].velocity = 0.0;//.fl_drive_speed = 0.0;
-  currentWheelStatus[FRONT_RIGHT_DRIVE_MOTOR_ID].velocity = 0.0;//.fr_drive_speed = 0.0;
-  currentWheelStatus[MIDDLE_LEFT_DRIVE_MOTOR_ID].velocity = 0.0;//.ml_drive_speed = 0.0;
-  currentWheelStatus[MIDDLE_RIGHT_DRIVE_MOTOR_ID].velocity = 0.0;//.mr_drive_speed = 0.0;
-  currentWheelStatus[REAR_LEFT_DRIVE_MOTOR_ID].velocity = 0.0;//.rl_drive_speed = 0.0;
-  currentWheelStatus[REAR_RIGHT_DRIVE_MOTOR_ID].velocity = 0.0;//.rr_drive_speed = 0.0;
-
-  // Publish our stuff
-  sabertoothDriverNode.advertise(pubResults);
-  sabertoothDriverNode.advertise(pubCurrentRotation);
+  setupWheelStatus();
+
 }
 
 // This program does whatever ROS directs it to do.
 // So far, it drives the robot forwards and backwards.
 void loop(){
+
+
+  //We are E-Stopped, don't respond to future commands.
+  if (currentStatus == E_STOPPED) {
+    return;
+  } 
+
+  //Currently stopped, don't do anything
+  if (currentStatus == STOPPED) {
+    //Do Nothing
+  }
+
+  if (currentStatus == ARTICULATING) {
+    if (needsToArticulate() == true) {
+       articulateAllWheels();
+    } else {
+       currentStatus = START_DRIVING;
+    }
+  }
+
+  if (currentStatus == START_DRIVING) {
+    //TODO: Check with MEs about possibility of losing correct articulation (by going over an obstacle or something)
+
+    //Check if we need to articulate
+    // if (needsToArticulate()) {
+    //   //Set state to articulate and don't drive
+    //   currentStatus = ARTICULATING;
+    //   return;
+    // }
+
+
+    //Set stop drive time
+    driveStopTime = millis() + wheelTarget.drive_duration*1000;
+
+    //Send drive start command
+    driveAllMotors();
+    
+    currentStatus = IS_DRIVING;
+  }
+
+  if (currentStatus == IS_DRIVING) {
+
+    if (millis() >= driveStopTime) {
+      stopAllMotors(false);
+    }
+
+
+  }
+
+  //Sync with ROS
+  sabertoothDriverNode.spinOnce(); // Check for subscriber update/update timestamp
+
+  //Delay so we don't overload any serial buffers
   delayMicroseconds(100000);
-  sabertoothDriverNode.spinOnce(); // Check for subscriber update/update timestamp
-  updateArticulationValues();
-  continueDriving();
-  // TODO: Delete this?
-  //  msg.data = (long)currentWheelStatus[FRONT_LEFT_DRIVE_MOTOR_ID].orientation;
-  //  pubResults.publish(&msg);
-
-}
-
-
+
+}
+
+

*/
void setup(){

  // Communicate with the computer
  EncoderFL::setupEncoderFL();
  EncoderML::setupEncoderML();
  EncoderRL::setupEncoderRL();

/* 
 / This is a simple Arduino program that demonstrates the
 / "Packetized Serial" method of using the Sabertooth Motor
