fixed shit ass linear climb

diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java
index 3b87c7e926144eac216f51a2e39a44fd1b8636d0..2cf72f11ef5d2609fbbd22e7d98459668288a433 100644 (file)
--- a/src/main/java/frc/robot/constants/Constants.java
+++ b/src/main/java/frc/robot/constants/Constants.java
@@ -16,7 +16,6 @@ public class Constants {
     public static final CANBus CANIVORE_CAN = new CANBus("CANivore");
     public static final CANBus CANIVORE_SUB = new CANBus("CANivoreSub");
     public static final CANBus RIO_CAN = new CANBus("rio");
-    public static final CANBus SUBSYSTEM_CANIVORE_CAN = new CANBus("CANivoreSub");
 
     // Logging 
     public static final boolean USE_TELEMETRY = true;

diff --git a/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java
index 768b4967e17c6463c6cdf52aac9975c2fee0db02..8d07719518e481debba0305eeffb66166743b103 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java
+++ b/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java
@@ -26,8 +26,6 @@ public class ClimbConstants {
     public final static double PID_TOLERANCE = 0.2;
 
     // Motor Limits
-    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

diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
index df405dce9e0bece2083b657755daa613d856e44f..412b42a19fbb5fe97c4bf0e8fc90674afe120e57 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
+++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
@@ -14,6 +14,7 @@ import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.Constants;
 import frc.robot.constants.IdConstants;
 
 public class LinearClimb extends SubsystemBase implements LinearClimbIO{
@@ -25,6 +26,8 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
 
     private boolean calibrateOnStartUp = false;
 
+    private double MAX_POWER = 0.2;
+
     private LinearClimbIOInputs inputs = new LinearClimbIOInputs();
 
     private final PIDController pid = new PIDController(
@@ -33,7 +36,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
             ClimbConstants.PID_D);
 
     public LinearClimb() {
-        motor = new TalonFX(IdConstants.CLIMB_MOTOR_ID);
+        motor = new TalonFX(IdConstants.CLIMB_MOTOR_ID, Constants.CANIVORE_SUB);
         pid.setTolerance(ClimbConstants.PID_TOLERANCE);
 
         motor.setNeutralMode(NeutralModeValue.Brake);
@@ -98,6 +101,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
      * goes to the up position
      */
     public void goUp() {
+        MAX_POWER = 0.8;
         setSetpoint((Units.radiansToRotations(upPosition / ClimbConstants.WHEEL_RADIUS)) * ClimbConstants.CLIMB_GEAR_RATIO);
     }
 
@@ -105,6 +109,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
      * goes to the down position
      */
     public void goDown() {
+        MAX_POWER = 0.2;
         setSetpoint((Units.radiansToRotations(downPosition / ClimbConstants.WHEEL_RADIUS)) * ClimbConstants.CLIMB_GEAR_RATIO);
     }
 
@@ -112,6 +117,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
      * goes to the climb position
      */
     public void climb() {
+        MAX_POWER = 0.8;
         setSetpoint((Units.radiansToRotations(climbPosition / ClimbConstants.WHEEL_RADIUS)) * ClimbConstants.CLIMB_GEAR_RATIO);
     }
 
@@ -120,7 +126,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
         // if it is not calibrating, do normal stuff
         if (!calibrating) {
             double power = pid.calculate(motor.getPosition().getValueAsDouble());
-            power = MathUtil.clamp(power, ClimbConstants.MIN_POWER, ClimbConstants.MAX_POWER);
+            power = MathUtil.clamp(power, -MAX_POWER, MAX_POWER);
             motor.set(power);
         } else {
             motor.set(ClimbConstants.CALIBRATION_POWER);
@@ -161,6 +167,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
         motor.setPosition((Units.radiansToRotations(downPosition / ClimbConstants.WHEEL_RADIUS)) * ClimbConstants.CLIMB_GEAR_RATIO);
         calibrating = false;
         setCurrentLimits(ClimbConstants.CLIMB_CURRENT);
+        setSetpoint((Units.radiansToRotations(downPosition / ClimbConstants.WHEEL_RADIUS)) * ClimbConstants.CLIMB_GEAR_RATIO);
     }
 
     @Override