From: Brian-Kletchikov-Nguyen <kleb9949@lgsstudent.org>
Date: Tue, 10 Feb 2026 00:49:23 +0000 (-0800)
Subject: Feedforward
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=91b97ab1c9d26efb6a9b84b5ddb58ef08c984aa5;p=FRC2026.git

Feedforward
---

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);
     }