From 347f83dbf53dbcb36d4aeb9ebd08df450bd34c8f Mon Sep 17 00:00:00 2001 From: nh17000 <46301909+nh17000@users.noreply.github.com> Date: Wed, 6 Aug 2025 11:42:24 -0500 Subject: [PATCH] Run SysID routines in sim --- src/main/java/frc/robot/Constants.java | 30 +++++++------ src/main/java/frc/robot/RobotContainer.java | 14 ++++++ .../java/frc/robot/subsystems/arm/Arm.java | 43 ++++++++++++++++++- .../java/frc/robot/subsystems/arm/ArmIO.java | 4 +- .../frc/robot/subsystems/arm/ArmIOSim.java | 8 ++-- .../robot/subsystems/arm/ArmIOTalonFX.java | 8 ++-- .../endeffector/EndEffectorIOSim.java | 2 +- 7 files changed, 83 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0e8e87f..681a542 100644 --- 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,12 +222,14 @@ 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" @@ -235,10 +237,12 @@ public static final TalonFXConfiguration getWristConfig() { 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 index d9c6511..733d819 100644 --- 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 index ee02994..57ddc78 100644 --- 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 index 06c4130..f3ef4e2 100644 --- 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 index b168576..df4de98 100644 --- 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 index ab15df1..b199bb1 100644 --- 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 index 8a32ea5..2dee385 100644 --- 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);