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

  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 (currentStatus == ACTUATING) {
    if(needsToActuate(currentExcavatorCommand.excavation_height)){
      moveToPercent(currentExcavatorCommand.excavation_height);
      excavatorStatus.excavate_speed = toPercent(currentSteps);
    }
    else{
      currentStatus = START_DIGGING;
    }
  }

