   excavatorStatus.drive_duration = 0;
   excavatorStatus.excavate_speed = 0;
   excavatorStatus.excavate_duration = 0;
-  excavatorStatus.excavation_height = 50;
+  excavatorStatus.excavation_height = 100;
   excavatorStatus.e_stop = false;
 }
 

    if (millis() >= driveStopTime) {
      print("over time limit: stop driving");
      stopMovementMotors();
      currentStatus = ACTUATING;
    }
  }

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

  if (currentStatus == START_DIGGING) {
    if(currentExcavatorCommand.excavate_speed == 0){
      stopBucketWheel();
    }
    else if(currentExcavatorCommand.excavate_speed < 0){
      bucketStopTime = millis() + (long)(currentExcavatorCommand.excavate_duration);   
      rotateBucketCounterclockwise(-currentExcavatorCommand.excavate_speed);
      excavatorStatus.excavate_speed = currentExcavatorCommand.excavate_speed;
    }
    else if(currentExcavatorCommand.excavate_speed > 0){
      bucketStopTime = millis() + (long)(currentExcavatorCommand.excavate_duration);   
      rotateBucketClockwise(currentExcavatorCommand.excavate_speed);
      excavatorStatus.excavate_speed = currentExcavatorCommand.excavate_speed;
    }
  }

  if (currentStatus == IS_DIGGING) {
    if(millis() >= bucketStopTime){
      stopBucketWheel();
    }
  }

  pubexcavatorStatus.publish(&excavatorStatus);// current rotation data for each wheel. 

  //Sync with ROS
  excavatorNode.spinOnce(); // Check for subscriber update/update timestamp


  delayMicroseconds(1000);
}

//<---------------CODE TO ACTUATE BUCKET WHEEL---------------->
int toSteps(int percent){
  return (percent / 100.0) * FULL_STEPS;
}

// checks whether we need to actuate the bucket wheel
// returns false if we are within +/- 3 steps
bool needsToActuate(int percent){
  int stepsToMove = int(-(toPercent(currentSteps) - percent) / 100.0 * FULL_STEPS);
  if(stepsToMove <= 3 && stepsToMove >= -3){
    return false;
  }
  return true; 
