diff --git a/src/main/java/frc/robot/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/elevator/ElevatorSubsystem.java index 06741c25..f332fcc6 100644 --- a/src/main/java/frc/robot/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/elevator/ElevatorSubsystem.java @@ -29,8 +29,6 @@ public class ElevatorSubsystem extends LifecycleSubsystem { private final LoggedDashboardNumber ntposition = new LoggedDashboardNumber("Elevator/positionOverride", -1); - private static final double PRE_MATCH_HOMING_MIN_MOVEMENT = 0.0; - private static final double TOLERANCE = 0.0; // Add to config @@ -38,9 +36,10 @@ public class ElevatorSubsystem extends LifecycleSubsystem { private static double maxHeight = RobotConfig.get().elevator().minHeight(); // Homing + double currentHeight = 0.0; + double homingEndPosition = RobotConfig.get().elevator().homingEndPosition(); private boolean preMatchHomingOccured = false; private double lowestSeenHeight = 0.0; - private double highestSeenHeight = 0.0; private HomingState homingState = HomingState.PRE_MATCH_HOMING; private double goalHeight = ElevatorPositions.STOWED; @@ -57,18 +56,15 @@ public ElevatorSubsystem(TalonFX motor) { @Override public void disabledPeriodic() { - double currentPosition = getHeight(); + currentHeight = getHeight(); - if (currentPosition < lowestSeenHeight) { - lowestSeenHeight = currentPosition; + if (currentHeight < lowestSeenHeight) { + lowestSeenHeight = currentHeight; } } @Override public void robotPeriodic() { - double rawCurrent = motor.getStatorCurrent().getValueAsDouble(); - double filteredCurrent = currentFilter.calculate(rawCurrent); - switch (homingState) { case NOT_HOMED: motor.setControl(coastNeutralRequest); @@ -79,8 +75,7 @@ public void robotPeriodic() { } else { motor.setControl(brakeNeutralRequest); - if (!preMatchHomingOccured && rangeOfMotionSeen()) { - // homingEndPosition + (currentAngle - minAngle) + if (!preMatchHomingOccured) { double homingEndPosition = RobotConfig.get().elevator().homingEndPosition(); double homedPosition = homingEndPosition + (getHeight() - lowestSeenHeight); motor.setPosition(inchesToRotations(homedPosition)); @@ -91,14 +86,13 @@ public void robotPeriodic() { } break; case MID_MATCH_HOMING: - motor.set(RobotConfig.get().elevator().homingVoltage()); - - if (filteredCurrent > RobotConfig.get().elevator().homingCurrentThreshold()) { - homingState = HomingState.HOMED; - goalHeight = ElevatorPositions.STOWED; + if (preMatchHomingOccured) { + double homedPosition = homingEndPosition + (getHeight() - lowestSeenHeight); + motor.setPosition(inchesToRotations(homedPosition)); - motor.setPosition(inchesToRotations(RobotConfig.get().elevator().homingEndPosition())); - } + preMatchHomingOccured = true; + homingState = HomingState.HOMED; + } break; case HOMED: double usedGoalPosition = @@ -114,10 +108,6 @@ public void robotPeriodic() { } } - public boolean rangeOfMotionSeen() { - return highestSeenHeight - lowestSeenHeight >= PRE_MATCH_HOMING_MIN_MOVEMENT; - } - public void setGoalHeight(double newHeight) { goalHeight = clampHeight(newHeight); }