 
   if (currentStatus == ACTUATING) {
     if(needsToActuate(currentExcavatorCommand.excavation_height)){
-       moveToPercent(currentExcavatorCommand.excavation_height);
-       excavatorStatus.excavate_speed = toPercent(currentSteps);
+      moveToPercent(currentExcavatorCommand.excavation_height);
+      excavatorStatus.excavate_speed = toPercent(currentSteps);
     }
     else{
-       currentStatus = START_DIGGING;
+      currentStatus = START_DIGGING;
     }
   }
 

  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;
  }
}



    //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();
