diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 42eb55b..5ab2eed 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -40,6 +40,8 @@ public final class Constants { public static final double ELEVATOR_DEADBAND_VALUE = .6; public static final double maxElevatorMotorSpeed = 0.3; //Normally .2 + public static final double ELEVATOR_SPEED_MOD_REDUCED = 0.5; + public static final double MINIMUM_ELEVATOR_POSITION = 2.2; public static final double restElevatorTargetPosition = 10.6; diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 5029d5d..1607334 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -34,6 +34,10 @@ public class ElevatorSubsystem extends SubsystemBase { public double newPower; public DigitalInput elevatorLimitSwitch; + + public int previousDirection; + + public double elevatorSpeedMod = 1; public ElevatorSubsystem(ClawIntakeSubsystem intake) { @@ -70,6 +74,7 @@ public ElevatorSubsystem(ClawIntakeSubsystem intake) public void setTargetPosition(double newTargetPosition, String reason) { targetPosition = newTargetPosition; + previousDirection = (int) (Math.signum(targetPosition - potentiometer.get())); } private double getCappedPower(double desired) @@ -86,6 +91,8 @@ public void setSpeed(double newSpeed) { speed *= Constants.maxElevatorMotorSpeed; + speed *= elevatorSpeedMod; + speed = getCappedPower(speed); double currentPower = elevatorMotorMain.get(); @@ -131,13 +138,22 @@ public void periodic() { } else { setSpeed(0); + elevatorSpeedMod = 1; } } + + if(previousDirection != Math.signum(targetPosition - potentiometer.get())) { + previousDirection = (int) (Math.signum(targetPosition - potentiometer.get())); + elevatorSpeedMod = Constants.ELEVATOR_SPEED_MOD_REDUCED; + } + if(potentiometer.get() < Constants.clawCloseThreshold && !clawThresholdOverridden && intake.clawGrabberLeft.get() != Value.kReverse && Robot.isEnabled) intake.setSolenoid(Value.kReverse); - + + + } public boolean isInTarget() {