fixes

diff --git a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
index 97ff2e6d78829f5d53ea496c2807c550dfeb33b2..8b358ad18974aee08bddea5eedca8e952b703eef 100644 (file)
--- a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
+++ b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
@@ -6,7 +6,7 @@ public class ClimbConstants {
     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;
-    public final static double MIN_HEIGHT = 6;
+    public final static double MIN_HEIGHT = 0;
     public final static double MAX_HEIGHT = 8;
     public final static double RADIUS = 0.3;
     public final static double CLIMB_HEIGHT = 4;

diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
index c872e4c91e750bff5bcf2899b3c1b15fbb5be44b..e27fb871f41d71784cd7489056d65341aae9a8a5 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
+++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
@@ -26,27 +26,10 @@ import frc.robot.constants.swerve.DriveConstants;
 public class LinearClimb extends SubsystemBase{
     private TalonFX motor;
 
-    private double rotationalSetpoint = 0;
-
     private static PIDController pid = new PIDController(0.1, 0, 0);
-
-    private double kP = 1.0;
-    private double kI = 0.0;
-    private double kD = 0.0;  
-
-    private double SpringResistance = -0.1;
-    private double RobotResistance = (DriveConstants.ROBOT_MASS * Constants.GRAVITY_ACCELERATION * ClimbConstants.RADIUS)/ClimbConstants.CLIMB_GEAR_RATIO;
-
-    private final DCMotor motorConstant = DCMotor.getKrakenX44(1);
     
     private double gearRatio = ClimbConstants.CLIMB_GEAR_RATIO;
 
-    private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(Units.degreesToRotations(0) * gearRatio); // gear ratio
-
-
-    // ElevatorFeedforward feedforward = new ElevatorFeedforward(0, (((DriveConstants.ROBOT_MASS * Constants.GRAVITY_ACCELERATION * ClimbConstants.RADIUS) / gearRatio) / motorConstant.KtNMPerAmp) * motorConstant.rOhms, 0, 0);
-    // ElevatorFeedforward feedforward = new ElevatorFeedforward(0, SpringResistance, 0, 0);
-
     public LinearClimb() {
         motor = new TalonFX(IdConstants.CLIMB_MOTOR_ID);//, Constants.RIO_CAN);
         pid.setTolerance(0.2);
@@ -62,21 +45,6 @@ public class LinearClimb extends SubsystemBase{
         config.CurrentLimits.SupplyCurrentLimitEnable = true;
         config.CurrentLimits.SupplyCurrentLimit = 5.0;
 
-        // config.Slot0.kS = 0.0; // Static friction compensation (should be >0 if friction exists)
-        // config.Slot0.kG = 0.0; // Gravity compensation
-        // config.Slot0.kV = 1; // Velocity gain: 1 rps -> 0.12V
-        // config.Slot0.kA = 0.3; // Acceleration gain: 1 rps² -> 0V (should be tuned if acceleration matters)
-        
-        // config.Slot0.kP = kP; // If position error is 1 rotation, apply 10V
-        // config.Slot0.kI = kI; // Integral term (usually left at 0 for MotionMagic)
-        // config.Slot0.kD = kD; // Derivative term (used to dampen oscillations)
-        
-        // MotionMagicConfigs motionMagicConfigs = config.MotionMagic;
-        // motionMagicConfigs.MotionMagicCruiseVelocity = Units.radiansToRotations(ClimbConstants.MAX_VELOCITY) * gearRatio; // max velocity * gear ratio
-        // motionMagicConfigs.MotionMagicAcceleration = Units.radiansToRotations(ClimbConstants.MAX_ACCELERATION) * gearRatio; // max Acceleration * gear ratio
-        // SmartDashboard.putNumber("velocity", Units.radiansToRotations(ClimbConstants.MAX_VELOCITY) * gearRatio);
-        // SmartDashboard.putNumber("acceleration", Units.radiansToRotations(ClimbConstants.MAX_ACCELERATION) * gearRatio);
-
         config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
         motor.getConfigurator().apply(config);
         SmartDashboard.putData("Go Up", new InstantCommand(() -> goUp()));
@@ -84,7 +52,6 @@ public class LinearClimb extends SubsystemBase{
         SmartDashboard.putData("Climb", new InstantCommand(() -> climb()));
         SmartDashboard.putNumber("Position", getPosition());
 
-        // motor.setPosition((ClimbConstants.MAX_HEIGHT/(2 * Math.PI * ClimbConstants.RADIUS)) * gearRatio);
         motor.setPosition(0);
     }
 
@@ -94,19 +61,10 @@ public class LinearClimb extends SubsystemBase{
     */
     public void setSetpoint(double setpoint) {
         pid.setSetpoint(setpoint);
-        // setpoint = MathUtil.clamp(setpoint, ClimbConstants.MIN_HEIGHT, ClimbConstants.MAX_HEIGHT);
-
-        // // rotationalSetpoint = (setpoint / (2 * Math.PI * ClimbConstants.RADIUS)) * gearRatio;
-        // rotationalSetpoint = -45/2.25;
-        // System.out.println(rotationalSetpoint);
-
-        // // motor.setControl(voltageRequest.withPosition(rotationalSetpoint).withFeedForward(feedforward.calculate(0)));
-        // motor.setControl(voltageRequest.withPosition(rotationalSetpoint));
     }
 
     public boolean atSetPoint(){
         return pid.atSetpoint();
-        // return Math.abs(motor.getPosition().getValueAsDouble() - rotationalSetpoint) < 3.0;
     }
 
     public double getPosition() {
@@ -114,12 +72,10 @@ public class LinearClimb extends SubsystemBase{
     }
 
     public void goUp() {
-        // feedforward.setKg(SpringResistance);
-        setSetpoint(1);
+        setSetpoint(45);
     }
 
     public void goDown() {
-        // feedforward.setKg(RobotResistance  - SpringResistance);
         setSetpoint(ClimbConstants.MIN_HEIGHT);
     }