fixes?

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 55ec0ada72ae47118169c1098b7eb82ee287a137..36a619e5d8218103c315cdc5802f0f12ea8bba6a 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -57,6 +57,7 @@ public class RobotContainer {
    * Different robots may have different subsystems.
    */
   public RobotContainer(RobotId robotId) {
+    SmartDashboard.putString("Robot ID", robotId.toString());
     // dispatch on the robot
     switch (robotId) {
       case TestBed1:

diff --git a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
index 64a64fb9d16da5a17ac289a8430e6b7307779648..32d6dea8b06722cb0ae24df644d1564cf94072c1 100644 (file)
--- a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
+++ b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
@@ -3,9 +3,9 @@ package frc.robot.constants.Climb;
 public class ClimbConstants {
     // CHANGE LATER
 
-    public final static double CLIMB_GEAR_RATIO = 25 / 1;
-    public final static double MAX_VELOCITY = 0.1;
-    public final static double MAX_ACCELERATION = 1;
+    public final static double CLIMB_GEAR_RATIO = 9.0 / 1 * 5.0 / 1;
+    public final static double MAX_VELOCITY = 2;
+    public final static double MAX_ACCELERATION = 10;
     public final static double MIN_HEIGHT = 0;
     public final static double MAX_HEIGHT = 8;
     public final static double RADIUS = 0.514098;

diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java
index 335f77edee15acd7038b80d5ac870b7d39f753fe..84e324882938d1d99a8bcffca17e2e13dca877f5 100644 (file)
--- a/src/main/java/frc/robot/constants/IdConstants.java
+++ b/src/main/java/frc/robot/constants/IdConstants.java
@@ -20,5 +20,5 @@ public class IdConstants {
     public static final int CANDLE_ID = 1;
 
     // Climb
-    public static final int CLIMB_MOTOR_ID = 7; // change left and right  IDs later
+    public static final int CLIMB_MOTOR_ID = 8; 
 }

diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
index c0c894d95c844680599bbe4a1ffd587af58925e4..07e16d23f7e877a24aa6ec55c49600ec6815516e 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
+++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
@@ -1,5 +1,6 @@
 package frc.robot.subsystems.Climb;
 
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
@@ -23,7 +24,7 @@ public class LinearClimb {
 
     private double rotationalSetpoint = 0;
 
-    private double kP = 0.5;
+    private double kP = 1.5;
     private double kI = 0.0;
     private double kD = 0.0;  
 
@@ -36,21 +37,29 @@ 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, SpringResistance, 0, 0);
+    // ElevatorFeedforward feedforward = new ElevatorFeedforward(0, SpringResistance, 0, 0);
 
     public LinearClimb() {
-        motor = new TalonFX(IdConstants.CLIMB_MOTOR_ID, Constants.RIO_CAN);
+        motor = new TalonFX(IdConstants.CLIMB_MOTOR_ID);//, Constants.RIO_CAN);
 
         motor.setPosition(Units.degreesToRotations(0) * gearRatio); // gear ratio
         motor.setNeutralMode(NeutralModeValue.Brake);
 
         TalonFXConfiguration config = new TalonFXConfiguration();
 
+        config.CurrentLimits = new CurrentLimitsConfigs();
+
+        config.CurrentLimits.StatorCurrentLimitEnable = true;
+        config.CurrentLimits.StatorCurrentLimit = 10.0;
+        config.CurrentLimits.SupplyCurrentLimitEnable = true;
+        config.CurrentLimits.SupplyCurrentLimit = 10.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 = 0.0; // Velocity gain: 1 rps -> 0.12V
-        config.Slot0.kA = 0.0; // Acceleration gain: 1 rps² -> 0V (should be tuned if acceleration matters)
+        config.Slot0.kV = 0.5; // Velocity gain: 1 rps -> 0.12V
+        config.Slot0.kA = 10.0; // 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)
@@ -74,7 +83,8 @@ public class LinearClimb {
 
         rotationalSetpoint = (setpoint / (2 * Math.PI * ClimbConstants.RADIUS)) * gearRatio;
 
-        motor.setControl(voltageRequest.withPosition(rotationalSetpoint).withFeedForward(feedforward.calculate(0)));
+        // motor.setControl(voltageRequest.withPosition(rotationalSetpoint).withFeedForward(feedforward.calculate(0)));
+        motor.setControl(voltageRequest.withPosition(rotationalSetpoint));
     }
 
     public boolean atSetPoint(){
@@ -86,12 +96,12 @@ public class LinearClimb {
     }
 
     public void goUp() {
-        feedforward.setKg(SpringResistance);
+        // feedforward.setKg(SpringResistance);
         setSetpoint(ClimbConstants.MAX_HEIGHT);
     }
 
     public void goDown() {
-        feedforward.setKg(RobotResistance  - SpringResistance);
+        // feedforward.setKg(RobotResistance  - SpringResistance);
         setSetpoint(ClimbConstants.MIN_HEIGHT);
     }