From 91b97ab1c9d26efb6a9b84b5ddb58ef08c984aa5 Mon Sep 17 00:00:00 2001 From: Brian-Kletchikov-Nguyen Date: Mon, 9 Feb 2026 16:49:23 -0800 Subject: [PATCH] Feedforward --- src/main/java/frc/robot/subsystems/Climb/LinearClimb.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java index 82eacd9..109d5f7 100644 --- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java +++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java @@ -27,7 +27,8 @@ public class LinearClimb { private double kI = 0.0; private double kD = 0.0; - private double resistance = -0.1; + 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); @@ -36,7 +37,7 @@ public class LinearClimb { 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, resistance, 0, 0); + ElevatorFeedforward feedforward = new ElevatorFeedforward(0, SpringResistance, 0, 0); public LinearClimb() { motor = new TalonFX(IdConstants.CLIMB_MOTOR_ID, Constants.RIO_CAN); @@ -83,10 +84,12 @@ public class LinearClimb { } public void goUp() { + feedforward.setKg(SpringResistance); setSetpoint(ClimbConstants.MAX_HEIGHT); } public void goDown() { + feedforward.setKg(RobotResistance - SpringResistance); setSetpoint(ClimbConstants.MIN_HEIGHT); } -- 2.39.5