clean up LinearClimb

diff --git a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
index 295735dd7e77f000400dec8e75e09b3ac5bb21a7..acfdf77b47996d1877bd74cd5f58470399df86fd 100644 (file)
--- a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
+++ b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
@@ -1,8 +1,8 @@
 package frc.robot.constants.Climb;
 
 public class ClimbConstants {
+    
     // CHANGE LATER
-
     public final static double CLIMB_GEAR_RATIO = 9.0 / 1 * 5.0 / 1;
     public final static double MAX_VELOCITY = 1;
     public final static double MAX_ACCELERATION = 0.3;
@@ -12,4 +12,20 @@ public class ClimbConstants {
     public final static double WEAK_CURRENT = 7.0;
     public final static double OFFSET = 100.0;
     public final static double CLIMB_OFFSET = 80.0;
+
+    // PID Constants
+    public final static double PID_P = 0.1;
+    public final static double PID_I = 0.0;
+    public final static double PID_D = 0.0;
+    public final static double PID_TOLERANCE = 0.2;
+
+    // Motor Limits
+    public final static double DEFAULT_CURRENT_LIMIT = 5.0;
+    public final static double MIN_POWER = -0.2;
+    public final static double MAX_POWER = 0.2;
+    public final static double CALIBRATION_POWER = 0.15;
+
+    // Calibration
+    public final static int CALIBRATION_COUNTER_LIMIT = 250;
+    public final static double CALIBRATION_POSITION_OFFSET = 1.0;
 }

diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
index c6dc360c70e766f3b06aafaf2e8786d0d6498048..ba99c2da84bc1e53b0f268b22b62e0c9ecefd346 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
+++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
@@ -15,24 +15,29 @@ import frc.robot.constants.IdConstants;
 import frc.robot.constants.Climb.ClimbConstants;
 
 public class LinearClimb extends SubsystemBase {
-    private TalonFX motor;
+    private final TalonFX motor;
     private boolean calibrating = false;
-    double counter = 0;
-    double downPosition = ClimbConstants.OFFSET;
-    double upPosition = 0;
-    double climbPosition = ClimbConstants.CLIMB_OFFSET;
+    private double counter = 0;
+    private double downPosition = ClimbConstants.OFFSET;
+    private double upPosition = 0;
+    private double climbPosition = ClimbConstants.CLIMB_OFFSET;
 
-    private static PIDController pid = new PIDController(0.1, 0, 0);
-
-    private double gearRatio = ClimbConstants.CLIMB_GEAR_RATIO;
+    private final PIDController pid = new PIDController(
+            ClimbConstants.PID_P,
+            ClimbConstants.PID_I,
+            ClimbConstants.PID_D);
 
     public LinearClimb() {
-        motor = new TalonFX(IdConstants.CLIMB_MOTOR_ID);// , Constants.RIO_CAN);
-        pid.setTolerance(0.2);
+        motor = new TalonFX(IdConstants.CLIMB_MOTOR_ID);
+        pid.setTolerance(ClimbConstants.PID_TOLERANCE);
 
         motor.setNeutralMode(NeutralModeValue.Brake);
 
-        setCurrentLimits(5.0);
+        TalonFXConfiguration config = new TalonFXConfiguration();
+        config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+        motor.getConfigurator().apply(config);
+
+        setCurrentLimits(ClimbConstants.DEFAULT_CURRENT_LIMIT);
 
         SmartDashboard.putData("Go Up", new InstantCommand(() -> goUp()));
         SmartDashboard.putData("Go Down", new InstantCommand(() -> goDown()));
@@ -73,16 +78,17 @@ public class LinearClimb extends SubsystemBase {
         setSetpoint(climbPosition);
     }
 
+    @Override
     public void periodic() {
-        if (calibrating == false) {
+        if (!calibrating) {
             double power = pid.calculate(motor.getPosition().getValueAsDouble());
-            power = MathUtil.clamp(power, -0.2, 0.2);
+            power = MathUtil.clamp(power, ClimbConstants.MIN_POWER, ClimbConstants.MAX_POWER);
             motor.set(power);
         } else {
-            if (counter > 250) {
+            if (counter > ClimbConstants.CALIBRATION_COUNTER_LIMIT) {
                 stopCalibrating();
             }
-            motor.set(0.15);
+            motor.set(ClimbConstants.CALIBRATION_POWER);
             counter += 1;
         }
         SmartDashboard.putNumber("Position", getPosition());
@@ -98,7 +104,6 @@ public class LinearClimb extends SubsystemBase {
         config.CurrentLimits.SupplyCurrentLimitEnable = true;
         config.CurrentLimits.SupplyCurrentLimit = limit;
 
-        config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
         motor.getConfigurator().apply(config);
     }
 
@@ -109,7 +114,7 @@ public class LinearClimb extends SubsystemBase {
     }
 
     public void stopCalibrating() {
-        downPosition = motor.getPosition().getValueAsDouble() - 1.0;
+        downPosition = motor.getPosition().getValueAsDouble() - ClimbConstants.CALIBRATION_POSITION_OFFSET;
         upPosition = downPosition - ClimbConstants.OFFSET;
         climbPosition = upPosition + ClimbConstants.CLIMB_OFFSET;
         setSetpoint(downPosition);