   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 driveAllMotors()
 {
-  if(currentStatus == START_DRIVING)
-  {
-    helpDrive(FRONT_LEFT_DRIVE_MOTOR_ID, currentExcavatorCommand.drive_speed);
-    helpDrive(FRONT_RIGHT_DRIVE_MOTOR_ID, currentExcavatorCommand.drive_speed);
-    helpDrive(BACK_LEFT_DRIVE_MOTOR_ID, currentExcavatorCommand.drive_speed);
-    helpDrive(BACK_RIGHT_DRIVE_MOTOR_ID, currentExcavatorCommand.drive_speed);
-    excavatorStatus.drive_speed = currentExcavatorCommand.drive_speed;
-    currentStatus = IS_DRIVING;
-  }
+  if (currentExcavatorCommand.drive_speed < 0){
+    for (int i = 0; i < 4; ++i)
+    {
+       driveForwards(i, -currentExcavatorCommand.drive_speed);
+       excavatorStatus.drive_speed = currentExcavatorCommand.drive_speed;
+    } 
+  }
+  else{
+    for (int i = 0; i < 4; ++i)
+    {
+      driveBackwards(i, currentExcavatorCommand.drive_speed);
+      excavatorStatus.drive_speed = currentExcavatorCommand.drive_speed;
+    } 
+  }
+
+  currentStatus = IS_DRIVING;
 }
 
 

  for (int motorID = 0; motorID < NUM_EXCAVATOR_MOTORS; ++motorID) {
    driveForwards(motorID, 0);
  }
  currentExcavatorCommand.drive_speed = 0;
}

/**

    //TODO: This will never happen right now, may want for competition though
    return;
  } 

  //Currently stopped, don't do anything.
  // ASSUMES the robot is stopped when currentStatus is set to STOPPED.
  if (currentStatus == STOPPED) {
    // do nothing
  }


  if(currentConveyorStatus == START_ROTATING_CONVEYOR){
    conveyorStopTime = millis() + (long)conveyorRotationTime*1000;   
    driveConveyor();
