nh17000 · nh17000 · Aug 6, 2025
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -141,17 +141,17 @@ public static final TalonFXConfiguration getPivotConfig() {
             config.CurrentLimits.StatorCurrentLimitEnable = true;
             config.CurrentLimits.StatorCurrentLimit = 25;
 
-            config.MotionMagic.MotionMagicCruiseVelocity = Units.degreesToRadians(1000) / PIVOT_P_COEFFICIENT;
-            config.MotionMagic.MotionMagicAcceleration = Units.degreesToRadians(600) / PIVOT_P_COEFFICIENT;
+            config.MotionMagic.MotionMagicCruiseVelocity = 50;
+            config.MotionMagic.MotionMagicAcceleration = 50;
 
             config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
             config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
 
             config.Slot0.kG = 0.0;
-            config.Slot0.kS = 0.0;
-            config.Slot0.kV = 0.0;
-            config.Slot0.kA = 0.0;
-            config.Slot0.kP = 0.6;
+            config.Slot0.kS = 0.0015;
+            config.Slot0.kV = 2.29 * PIVOT_P_COEFFICIENT;
+            config.Slot0.kA = 0.05 * PIVOT_P_COEFFICIENT; // 0.023-0.084
+            config.Slot0.kP = 0.5;
             config.Slot0.kI = 0.0;
             config.Slot0.kD = 0.0;
 
@@ -166,16 +166,16 @@ public static final TalonFXConfiguration getExtensionConfig() {
             config.CurrentLimits.StatorCurrentLimitEnable = true;
             config.CurrentLimits.StatorCurrentLimit = 30;
 
-            config.MotionMagic.MotionMagicCruiseVelocity = Units.inchesToMeters(200) / EXTENSION_P_COEFFICIENT;
-            config.MotionMagic.MotionMagicAcceleration = Units.inchesToMeters(400) / EXTENSION_P_COEFFICIENT;
+            config.MotionMagic.MotionMagicCruiseVelocity = 110;
+            config.MotionMagic.MotionMagicAcceleration = 150;
 
             config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
             config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
 
             config.Slot0.kG = 0.0;
-            config.Slot0.kS = 0.0;
-            config.Slot0.kV = 0.0;
-            config.Slot0.kA = 0.0;
+            config.Slot0.kS = 0.0045;
+            config.Slot0.kV = 0.1185;
+            config.Slot0.kA = 0.00137;
             config.Slot0.kP = 0.5;
             config.Slot0.kI = 0.0;
             config.Slot0.kD = 0.0;
@@ -222,23 +222,27 @@ public static final TalonFXConfiguration getWristConfig() {
         public static final double EXTENSION_GEAR_RATIO = (32. / 16.) * (40. / 26.) * (50. / 20.) * (62. / 76.); // 6.28
         public static final double WRIST_GEAR_RATIO = (50. / 9.) * (38. / 12.) * (38. / 12.); // 55.71
 
-        public static final double ARM_MASS_KG = Units.lbsToKilograms(20);
         public static final double ARM_SHOULDER_TO_WRIST_LENGTH = VisualizerConstants.WRIST_OFFSET.getNorm();
 
         public static final double PIVOT_P_COEFFICIENT = 2 * Math.PI / PIVOT_GEAR_RATIO;
         public static final double PIVOT_MIN_ANGLE = 0;
         public static final double PIVOT_MAX_ANGLE = Units.degreesToRadians(120);
+        public static final double PIVOT_MASS = Units.lbsToKilograms(32);
+        public static final double PIVOT_MIN_KG = 0.211; // * PIVOT_P_COEFFICIENT; // at min extension
+        public static final double PIVOT_MAX_KG = 0.555; // * PIVOT_P_COEFFICIENT; // at max extension
         public static final DCMotor PIVOT_MOTORS = DCMotor.getKrakenX60(3);
 
         public static final double EXTENSION_DRUM_RADIUS = Units.inchesToMeters(0.25 * 16.0 / Math.PI * 0.5); // ~0.64"
         public static final double EXTENSION_P_COEFFICIENT =
                 2.0 * Math.PI * EXTENSION_DRUM_RADIUS / EXTENSION_GEAR_RATIO;
         public static final double EXTENSION_MIN_LENGTH = 0;
         public static final double EXTENSION_MAX_LENGTH = Units.inchesToMeters(42);
+        public static final double EXTENSION_MASS = Units.lbsToKilograms(17);
+        public static final double EXTENSION_KG = 0.11027;
         public static final DCMotor EXTENSION_MOTORS = DCMotor.getKrakenX60(3);
 
         public static final double WRIST_P_COEFFICIENT = 2 * Math.PI / WRIST_GEAR_RATIO;
-        public static final double WRIST_MASS_KG = Units.lbsToKilograms(2);
+        public static final double WRIST_MASS = Units.lbsToKilograms(2); // 12
         public static final double WRIST_LENGTH = Units.inchesToMeters(6);
         public static final double WRIST_STARTING_ANGLE = Units.degreesToRadians(125);
         public static final double WRIST_MIN_ANGLE = Units.degreesToRadians(-90);

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -218,6 +218,20 @@ private void configureButtonBindings() {
                 .and(() -> arm.getState() != ArmState.NET_PREP)
                 .whileTrue(endEffector.outtake(align::isForwards));
 
+        // controller.leftBumper().onTrue(Commands.runOnce(SignalLogger::start));
+        // controller.rightBumper().onTrue(Commands.runOnce(SignalLogger::stop));
+
+        /*
+         * Joystick Y = quasistatic forward
+         * Joystick A = quasistatic reverse
+         * Joystick B = dynamic forward
+         * Joystick X = dyanmic reverse
+         */
+        // opController.a().whileTrue(arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+        // opController.x().whileTrue(arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+        // opController.b().whileTrue(arm.sysIdDynamic(SysIdRoutine.Direction.kForward));
+        // opController.y().whileTrue(arm.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+
         // --- Operator Controls ---
         opController.leftBumper().onTrue(new InstantCommand(() -> endEffector.setMode(GamePieceMode.VERTICAL_CORAL)));
         opController.rightBumper().onTrue(new InstantCommand(() -> endEffector.setMode(GamePieceMode.ALGAE)));

diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java
@@ -1,8 +1,14 @@
 package frc.robot.subsystems.arm;
 
+import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.Volts;
+
+import com.ctre.phoenix6.SignalLogger;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants.ArmConstants;
 import frc.robot.Constants.ArmConstants.ArmState;
 import frc.robot.util.LoggedTunableNumber;
@@ -20,11 +26,22 @@ public class Arm extends SubsystemBase {
     private static final LoggedTunableNumber tunableExtension = new LoggedTunableNumber("Arm/Tunable Extension", 0.0);
     private static final LoggedTunableNumber tunableWrist = new LoggedTunableNumber("Arm/Tunable Wrist", 0.0);
 
+    private final SysIdRoutine pivotSysID;
+
     private ArmIO io;
     private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged();
 
     public Arm(ArmIO io) {
         this.io = io;
+
+        pivotSysID = new SysIdRoutine(
+                new SysIdRoutine.Config(
+                        Volts.per(Second).of(0.2), // Use default ramp rate (1 V/s)
+                        Volts.of(1), // Reduce dynamic step voltage to 4 to prevent brownout
+                        Second.of(5), // Use default timeout (10 s)
+                        // Log state with Phoenix SignalLogger class
+                        (state) -> SignalLogger.writeString("state", state.toString())),
+                new SysIdRoutine.Mechanism((volts) -> io.setWristVolts(volts.in(Volts)), null, this));
     }
 
     @Override
@@ -40,8 +57,8 @@ public void periodic() {
                     Units.degreesToRadians(tunableWrist.get()));
         }
 
-        io.setPivotSetpoint(setpoint.getPivotRads() / ArmConstants.PIVOT_P_COEFFICIENT);
-        io.setExtensionSetpoint(setpoint.getExtensionMeters() / ArmConstants.EXTENSION_P_COEFFICIENT);
+        io.setPivotSetpoint(setpoint.getPivotRads() / ArmConstants.PIVOT_P_COEFFICIENT, getPivotkG());
+        io.setExtensionSetpoint(setpoint.getExtensionMeters() / ArmConstants.EXTENSION_P_COEFFICIENT, getExtensionkG());
         io.setWristSetpoint(setpoint.getWristRads() / ArmConstants.WRIST_P_COEFFICIENT);
 
         Logger.recordOutput("Arm/Pivot Setpoint Rots", setpoint.getPivotRads() / ArmConstants.PIVOT_P_COEFFICIENT);
@@ -51,6 +68,14 @@ public void periodic() {
         Logger.recordOutput("Arm/Component Setpoints", state.position.getComponentTransforms());
     }
 
+    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+        return pivotSysID.quasistatic(direction);
+    }
+
+    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+        return pivotSysID.dynamic(direction);
+    }
+
     public Command applyState(ArmState desiredState) {
         return runOnce(() -> this.state = desiredState);
     }
@@ -77,4 +102,18 @@ public double getWristAngleRads() {
     public ArmPosition getArmPosition() {
         return new ArmPosition(getPivotAngleRads(), getExtensionLengthMeters(), getWristAngleRads());
     }
+
+    @AutoLogOutput
+    private double getPivotkG() {
+        // extension percentage, uses the setpoint to minimize overshoot when both move
+        double t = state.position.getExtensionMeters() / ArmState.NET.position.getExtensionMeters();
+
+        return MathUtil.interpolate(ArmConstants.PIVOT_MIN_KG, ArmConstants.PIVOT_MAX_KG, t)
+                * Math.cos(state.position.getPivotRads());
+    }
+
+    @AutoLogOutput
+    private double getExtensionkG() {
+        return ArmConstants.EXTENSION_KG * Math.sin(getPivotAngleRads());
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java
@@ -13,9 +13,9 @@ static class ArmIOInputs {
 
     default void updateInputs(ArmIOInputs inputs) {}
 
-    default void setPivotSetpoint(double setpointRots) {}
+    default void setPivotSetpoint(double setpointRots, double ffVolts) {}
 
-    default void setExtensionSetpoint(double setpointRots) {}
+    default void setExtensionSetpoint(double setpointRots, double ffVolts) {}
 
     default void setWristSetpoint(double setpointRots) {}
 

diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java
@@ -20,13 +20,13 @@ public class ArmIOSim extends ArmIOTalonFX {
             ArmConstants.ARM_SHOULDER_TO_WRIST_LENGTH,
             ArmConstants.PIVOT_MIN_ANGLE,
             ArmConstants.PIVOT_MAX_ANGLE,
-            ArmConstants.ARM_MASS_KG,
+            ArmConstants.PIVOT_MASS,
             true);
 
     private final TiltedElevatorSim tiltedElevatorSim = new TiltedElevatorSim(
             ArmConstants.EXTENSION_MOTORS,
             ArmConstants.EXTENSION_GEAR_RATIO,
-            ArmConstants.ARM_MASS_KG,
+            ArmConstants.EXTENSION_MASS,
             ArmConstants.EXTENSION_DRUM_RADIUS,
             ArmConstants.EXTENSION_MIN_LENGTH,
             ArmConstants.EXTENSION_MAX_LENGTH,
@@ -35,7 +35,7 @@ public class ArmIOSim extends ArmIOTalonFX {
     private final SingleJointedArmSim wristSim = new SingleJointedArmSim(
             ArmConstants.WRIST_MOTOR,
             ArmConstants.WRIST_GEAR_RATIO,
-            ArmConstants.WRIST_MASS_KG,
+            ArmConstants.WRIST_MASS,
             ArmConstants.WRIST_LENGTH,
             ArmConstants.WRIST_MIN_ANGLE,
             ArmConstants.WRIST_MAX_ANGLE,
@@ -93,7 +93,7 @@ private void updateSim() {
 
     @AutoLogOutput(key = "Arm/Estimated MOI")
     private double calcPivotMOI(double extensionLength) {
-        double m = ArmConstants.ARM_MASS_KG;
+        double m = ArmConstants.PIVOT_MASS;
         double r = ArmConstants.ARM_SHOULDER_TO_WRIST_LENGTH + extensionLength;
         return 1.0 / 3.0 * m * r * r;
     }

diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOTalonFX.java b/src/main/java/frc/robot/subsystems/arm/ArmIOTalonFX.java
@@ -46,13 +46,13 @@ public void updateInputs(ArmIOInputs inputs) {
     }
 
     @Override
-    public void setPivotSetpoint(double setpointRots) {
-        pivotOne.setControl(pivotMMRequest.withPosition(setpointRots));
+    public void setPivotSetpoint(double setpointRots, double ffVolts) {
+        pivotOne.setControl(pivotMMRequest.withPosition(setpointRots).withFeedForward(ffVolts));
     }
 
     @Override
-    public void setExtensionSetpoint(double setpointRots) {
-        extensionOne.setControl(extensionMMRequest.withPosition(setpointRots));
+    public void setExtensionSetpoint(double setpointRots, double ffVolts) {
+        extensionOne.setControl(extensionMMRequest.withPosition(setpointRots).withFeedForward(ffVolts));
     }
 
     @Override

diff --git a/src/main/java/frc/robot/subsystems/endeffector/EndEffectorIOSim.java b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorIOSim.java
@@ -45,7 +45,7 @@ public class EndEffectorIOSim extends EndEffectorIOTalonFX {
 
     private static final double MIN_POS = -0.1;
     private static final double MAX_POS = 0.35;
-    private static final double INTAKE_POS = 0.25;
+    private static final double INTAKE_POS = 0.33;
     private static final double POS_TOLERANCE = 0.01;
 
     private static final double CORAL_LENGTH = Units.inchesToMeters(11.875);