Feedforward

diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
index 82eacd9daac60d508dcc12316a6625beb64b5434..109d5f7e6f9a832e4b32e19ba0b64df2ad5658b6 100644 (file)
--- 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);
     }