From 7788cd9a5c46488ed67b1dbb1ec8fad5c6086cb5 Mon Sep 17 00:00:00 2001 From: henrymilbert <110063972+henrymilbert@users.noreply.github.com> Date: Wed, 25 Mar 2026 21:51:35 -0500 Subject: [PATCH 1/4] use SensorToMechanismRatio and setPosition for calibration for turret --- .../lib/hardware/components/GhostDevice.java | 7 +- .../lib/hardware/components/motor/IMotor.java | 1 + .../lib/hardware/factory/RobotFactory.java | 16 ++++ .../team1816/season/subsystems/Shooter.java | 92 ++++++++----------- .../schemas/hardware/deviceconfig.schema.json | 6 +- src/main/resources/yaml/ztldr.yml | 8 +- 6 files changed, 69 insertions(+), 61 deletions(-) diff --git a/src/main/java/com/team1816/lib/hardware/components/GhostDevice.java b/src/main/java/com/team1816/lib/hardware/components/GhostDevice.java index 39390844..ba1ee1b7 100644 --- a/src/main/java/com/team1816/lib/hardware/components/GhostDevice.java +++ b/src/main/java/com/team1816/lib/hardware/components/GhostDevice.java @@ -8,8 +8,6 @@ public class GhostDevice implements IMotor { - private int Id = 0; - public GhostDevice(int deviceID, CANBus canbus) { } @@ -45,8 +43,11 @@ public double getMotorPosition() { } @Override - public void zeroMotorPosition() { + public void zeroMotorPosition() {} + @Override + public StatusCode setPosition(double newValue) { + return StatusCode.OK; } @Override diff --git a/src/main/java/com/team1816/lib/hardware/components/motor/IMotor.java b/src/main/java/com/team1816/lib/hardware/components/motor/IMotor.java index cf1a4895..35d64edc 100644 --- a/src/main/java/com/team1816/lib/hardware/components/motor/IMotor.java +++ b/src/main/java/com/team1816/lib/hardware/components/motor/IMotor.java @@ -11,6 +11,7 @@ public interface IMotor extends IPhoenix6 { double getMotorPosition(); // used to zero the motor position void zeroMotorPosition(); + StatusCode setPosition(double newValue); StatusCode setSimRotorVelocity(double rps); StatusCode setSimRotorPosition(double rotations); double getSimMotorVoltage(); diff --git a/src/main/java/com/team1816/lib/hardware/factory/RobotFactory.java b/src/main/java/com/team1816/lib/hardware/factory/RobotFactory.java index 2ff6c227..1f09bbc7 100644 --- a/src/main/java/com/team1816/lib/hardware/factory/RobotFactory.java +++ b/src/main/java/com/team1816/lib/hardware/factory/RobotFactory.java @@ -487,6 +487,12 @@ private FeedbackConfigs GetFeedbackConfigs(DeviceConfiguration deviceConfig) { config.RotorToSensorRatio = deviceConfig.rotorToSensor; GreenLogger.log(" rotorToSensor: " + config.RotorToSensorRatio); } + + if (deviceConfig.sensorToMechanismRatio != null) { + config.SensorToMechanismRatio = deviceConfig.sensorToMechanismRatio; + GreenLogger.log(" sensorToMechanismRatio: " + config.SensorToMechanismRatio); + } + return config; } @@ -511,6 +517,16 @@ private ExternalFeedbackConfigs getExternalFeedbackConfigs(DeviceConfiguration d GreenLogger.log(" remoteOffest: " + config.AbsoluteSensorOffset); } + if (deviceConfig.rotorToSensor != null) { + config.RotorToSensorRatio = deviceConfig.rotorToSensor; + GreenLogger.log(" rotorToSensor: " + config.RotorToSensorRatio); + } + + if (deviceConfig.sensorToMechanismRatio != null) { + config.SensorToMechanismRatio = deviceConfig.sensorToMechanismRatio; + GreenLogger.log(" sensorToMechanismRatio: " + config.SensorToMechanismRatio); + } + return config; } diff --git a/src/main/java/com/team1816/season/subsystems/Shooter.java b/src/main/java/com/team1816/season/subsystems/Shooter.java index ff5dd82f..94b81ad3 100644 --- a/src/main/java/com/team1816/season/subsystems/Shooter.java +++ b/src/main/java/com/team1816/season/subsystems/Shooter.java @@ -110,7 +110,6 @@ public class Shooter extends SubsystemBase implements ITestableSubsystem { private boolean previousSensorValuesKnown = false; //CONSTANTS - private final double MOTOR_ROTATIONS_PER_TURRET_ROTATION; private final Translation3d SHOOTER_OFFSET; /** * The tolerance for the turret rotation to consider it aimed at the target (in degrees). @@ -130,29 +129,29 @@ public class Shooter extends SubsystemBase implements ITestableSubsystem { */ private final double INCLINE_DUCKING_LIMIT_ROTATIONS; /** - * The turret position opposite the dead zone, in turret rotations. + * The turret position opposite the dead zone, (in rotations). */ - private final double OPPOSITE_OF_DEAD_ZONE_TURRET_ROTATIONS; + private final double OPPOSITE_OF_DEAD_ZONE_POSITION_ROTATIONS; /** * The first (in the counterclockwise direction) of the four positions of the turret motor - * where we would see beam break values change, in motor rotations. + * where we would see beam break values change, (in rotations). */ - private final double FIRST_BEAM_BREAK_POSITION_MOTOR_ROTATIONS; + private final double FIRST_BEAM_BREAK_POSITION_ROTATIONS; /** * The second (in the counterclockwise direction) of the four positions of the turret motor - * where we would see beam break values change, in motor rotations. + * where we would see beam break values change, (in rotations). */ - private final double SECOND_BEAM_BREAK_POSITION_MOTOR_ROTATIONS; + private final double SECOND_BEAM_BREAK_POSITION_ROTATIONS; /** * The third (in the counterclockwise direction) of the four positions of the turret motor - * where we would see beam break values change, in motor rotations. + * where we would see beam break values change, (in rotations). */ - private final double THIRD_BEAM_BREAK_POSITION_MOTOR_ROTATIONS; + private final double THIRD_BEAM_BREAK_POSITION_ROTATIONS; /** * The fourth (in the counterclockwise direction) of the four positions of the turret motor - * where we would see beam break values change, in motor rotations. + * where we would see beam break values change, (in rotations). */ - private final double FOURTH_BEAM_BREAK_POSITION_MOTOR_ROTATIONS; + private final double FOURTH_BEAM_BREAK_POSITION_ROTATIONS; /** * The amount by which to increase or decrease the {@link #launchVelocityAdjustmentRPS} per * call to {@link #increaseLaunchVelocityAdjustment()} or {@link @@ -178,11 +177,6 @@ public class Shooter extends SubsystemBase implements ITestableSubsystem { private final double TOP_LAUNCH_MOTOR_BACKSPIN_MULTIPLIER; //CALIBRATION - /** - * The offset in motor rotations of the turret motor from the reference frame where robot - * forward is zero to the positions read by the motor. - */ - private double turretMotorOffsetRotations; private final double FAST_CALIBRATION_SPEED = 0.09; private final double SLOW_CALIBRATION_SPEED = 0.04; private final DutyCycleOut turretDutyCycleOutRequest = new DutyCycleOut(0); @@ -210,27 +204,24 @@ public Shooter() { super(); // if the turret is ghosted we can say we are calibrated because the motors will not move if(turretMotor.isGhost()) isTurretCalibrated = true; - MOTOR_ROTATIONS_PER_TURRET_ROTATION = factory.getConstant(NAME, "motorRotationsPerTurretRotation", 1); SHOOTER_OFFSET = new Translation3d( factory.getConstant(NAME, "shooterOffsetXMeters",0), factory.getConstant(NAME, "shooterOffsetYMeters",0), factory.getConstant(NAME, "shooterOffsetZMeters",0) ); - // Find the turret positions of the four spots that we would see beam break values change, - // in motor rotations relative robot forward. + // Find the turret positions of the four spots that we would see beam break values change. double closeDistanceBetweenBeamBreaks = factory.getConstant(NAME, "closeDistanceBetweenBeamBreaks", 0); double farDistanceBetweenBeamBreaks = factory.getConstant(NAME, "farDistanceBetweenBeamBreaks", 0); double secondLowestBeamBreakToZero = factory.getConstant(NAME, "secondLowestBeamBreakToZero", 0); - FIRST_BEAM_BREAK_POSITION_MOTOR_ROTATIONS = -secondLowestBeamBreakToZero - closeDistanceBetweenBeamBreaks; - SECOND_BEAM_BREAK_POSITION_MOTOR_ROTATIONS = -secondLowestBeamBreakToZero; - THIRD_BEAM_BREAK_POSITION_MOTOR_ROTATIONS = -secondLowestBeamBreakToZero + farDistanceBetweenBeamBreaks; - FOURTH_BEAM_BREAK_POSITION_MOTOR_ROTATIONS = -secondLowestBeamBreakToZero + farDistanceBetweenBeamBreaks + closeDistanceBetweenBeamBreaks; + FIRST_BEAM_BREAK_POSITION_ROTATIONS = -secondLowestBeamBreakToZero - closeDistanceBetweenBeamBreaks; + SECOND_BEAM_BREAK_POSITION_ROTATIONS = -secondLowestBeamBreakToZero; + THIRD_BEAM_BREAK_POSITION_ROTATIONS = -secondLowestBeamBreakToZero + farDistanceBetweenBeamBreaks; + FOURTH_BEAM_BREAK_POSITION_ROTATIONS = -secondLowestBeamBreakToZero + farDistanceBetweenBeamBreaks + closeDistanceBetweenBeamBreaks; // Get the position opposite of the dead zone to use as the center of our wrapped range. - double oppositeOfDeadZoneMotorRotations = ( - SECOND_BEAM_BREAK_POSITION_MOTOR_ROTATIONS + THIRD_BEAM_BREAK_POSITION_MOTOR_ROTATIONS + OPPOSITE_OF_DEAD_ZONE_POSITION_ROTATIONS = ( + SECOND_BEAM_BREAK_POSITION_ROTATIONS + THIRD_BEAM_BREAK_POSITION_ROTATIONS ) / 2; - OPPOSITE_OF_DEAD_ZONE_TURRET_ROTATIONS = oppositeOfDeadZoneMotorRotations / MOTOR_ROTATIONS_PER_TURRET_ROTATION; TURRET_ROTATION_TOLERANCE_DEGREES = factory.getConstant(NAME, "turretRotationToleranceDegrees", 0); INCLINE_ANGLE_TOLERANCE_DEGREES = factory.getConstant(NAME, "inclineAngleToleranceDegrees", 0); @@ -270,7 +261,6 @@ public Shooter() { GreenLogger.periodicLog(NAME + "/turret/Aiming in Dead Zone", () -> isTurretAimingInDeadZone); GreenLogger.periodicLog(NAME + "/turret/Left Sensor Triggered", () -> leftSensorTriggered); GreenLogger.periodicLog(NAME + "/turret/Right Sensor Triggered", () -> rightSensorTriggered); - GreenLogger.periodicLog(NAME + "/turret/Motor Offset Rotations", () -> turretMotorOffsetRotations); GreenLogger.periodicLog(NAME + "/turret/Fixed Angle Degrees", () -> turretFixedAngleDegrees); GreenLogger.periodicLog(NAME + "/turret/Auto Aiming Turret", () -> autoAimTurret); GreenLogger.periodicLog(NAME + "/turret/Angle Adjustment Degrees", () -> turretAngleAdjustmentDegrees); @@ -527,8 +517,8 @@ private void aimInclineAndLaunchersAtTarget(Translation2d targetTranslation2d) { } /** - * Determines the offset of the {@link #turretMotor} based on where the beam break - * sensors change triggered values. + * Moves the turret until the beam break sensors change triggered values and uses this to zero + * the {@link #turretMotor}. */ private void calibrateTurretMotor() { // We only want to run the motors to automatically calibrate if the robot is enabled, but the @@ -568,28 +558,29 @@ else if (rightSensorTriggered) { if (leftSensorTriggered != previousLeftSensorTriggered) { // Change in left sensor triggered value. if (rightSensorTriggered) { // Must be at the first of the four positions where sensor values change. - finishCalibration(FIRST_BEAM_BREAK_POSITION_MOTOR_ROTATIONS); + finishCalibration(FIRST_BEAM_BREAK_POSITION_ROTATIONS); } else { // Must be at the third of the four positions where sensor values change. - finishCalibration(THIRD_BEAM_BREAK_POSITION_MOTOR_ROTATIONS); + finishCalibration(THIRD_BEAM_BREAK_POSITION_ROTATIONS); } } else if (rightSensorTriggered != previousRightSensorTriggered) { // Change in the right sensor triggered value. if (leftSensorTriggered) { // Must be at the fourth of the four positions where sensor values change. - finishCalibration(FOURTH_BEAM_BREAK_POSITION_MOTOR_ROTATIONS); + finishCalibration(FOURTH_BEAM_BREAK_POSITION_ROTATIONS); } else { // Must be at the second of the four positions where sensor values change. - finishCalibration(SECOND_BEAM_BREAK_POSITION_MOTOR_ROTATIONS); + finishCalibration(SECOND_BEAM_BREAK_POSITION_ROTATIONS); } } } } - private void finishCalibration(double beamBreakPositionMotorRotations) { - turretMotorOffsetRotations = turretMotor.getMotorPosition() - beamBreakPositionMotorRotations; + private void finishCalibration(double beamBreakPositionRotations) { + // Tell the turret motor that it is now at the specific beam break position. + turretMotor.setPosition(beamBreakPositionRotations); turretMotor.setControl(turretDutyCycleOutRequest.withOutput(0)); isTurretCalibrated = true; } @@ -684,28 +675,25 @@ private boolean isInclineAimed() { private void setTurretAngle(double wantedAngleDegrees) { wantedTurretAngleDegrees = wantedAngleDegrees + turretAngleAdjustmentDegrees; if (isTurretCalibrated) { - double wantedTurretRotations = Units.degreesToRotations(wantedTurretAngleDegrees); + double wantedRotations = Units.degreesToRotations(wantedTurretAngleDegrees); - double wrappedWantedTurretRotations = MathUtil.inputModulus( - wantedTurretRotations, - OPPOSITE_OF_DEAD_ZONE_TURRET_ROTATIONS - 0.5, - OPPOSITE_OF_DEAD_ZONE_TURRET_ROTATIONS + 0.5 + double wrappedWantedRotations = MathUtil.inputModulus( + wantedRotations, + OPPOSITE_OF_DEAD_ZONE_POSITION_ROTATIONS - 0.5, + OPPOSITE_OF_DEAD_ZONE_POSITION_ROTATIONS + 0.5 ); - double wantedMotorRotations = wrappedWantedTurretRotations * MOTOR_ROTATIONS_PER_TURRET_ROTATION; // Clamp the position between the first and fourth beam break positions. - double lowerLimitMotorRotations = FIRST_BEAM_BREAK_POSITION_MOTOR_ROTATIONS; - double upperLimitMotorRotations = FOURTH_BEAM_BREAK_POSITION_MOTOR_ROTATIONS; - isTurretAimingInDeadZone = wantedMotorRotations < lowerLimitMotorRotations || wantedMotorRotations > upperLimitMotorRotations; + double lowerLimitRotations = FIRST_BEAM_BREAK_POSITION_ROTATIONS; + double upperLimitRotations = FOURTH_BEAM_BREAK_POSITION_ROTATIONS; + isTurretAimingInDeadZone = wrappedWantedRotations < lowerLimitRotations || wrappedWantedRotations > upperLimitRotations; double clampedMotorRotations = MathUtil.clamp( - wantedMotorRotations, - lowerLimitMotorRotations, - upperLimitMotorRotations + wrappedWantedRotations, + lowerLimitRotations, + upperLimitRotations ); - turretMotor.setControl(turretMotorPositionRequest.withPosition( - clampedMotorRotations + turretMotorOffsetRotations - )); + turretMotor.setControl(turretMotorPositionRequest.withPosition(clampedMotorRotations)); } } @@ -734,9 +722,7 @@ private Pose2d getCurrentTurretPose2d() { * @return The current robot-relative {@link Rotation2d} of the turret. */ private Rotation2d getCurrentRobotRelativeTurretRotation2d() { - double motorRotations = turretMotor.getMotorPosition(); - double offsetMotorRotations = motorRotations - turretMotorOffsetRotations; - double turretRotations = offsetMotorRotations / MOTOR_ROTATIONS_PER_TURRET_ROTATION; + double turretRotations = turretMotor.getMotorPosition(); return Rotation2d.fromRotations(turretRotations); } diff --git a/src/main/resources/schemas/hardware/deviceconfig.schema.json b/src/main/resources/schemas/hardware/deviceconfig.schema.json index e15d5a45..77bbd601 100644 --- a/src/main/resources/schemas/hardware/deviceconfig.schema.json +++ b/src/main/resources/schemas/hardware/deviceconfig.schema.json @@ -82,7 +82,11 @@ "additionalProperties": false }, "rotorToSensor": { - "default": 1, + "default": null, + "type": "number" + }, + "sensorToMechanismRatio": { + "default": null, "type": "number" } } diff --git a/src/main/resources/yaml/ztldr.yml b/src/main/resources/yaml/ztldr.yml index 73c7c365..e61fa073 100644 --- a/src/main/resources/yaml/ztldr.yml +++ b/src/main/resources/yaml/ztldr.yml @@ -272,6 +272,7 @@ subsystems: deviceType: TalonFX id: 28 motorRotation: Clockwise_Positive + sensorToMechanismRatio: 15.625 # 250/16 gear ratio pidConfig: slot0: kS: 0.29 @@ -290,7 +291,6 @@ subsystems: constants: leftTurretSensorChannel: 0 rightTurretSensorChannel: 1 - motorRotationsPerTurretRotation: 15.625 # 250/16 gear ratio shooterOffsetXMeters: -0.11303 shooterOffsetYMeters: -0.112268 shooterOffsetZMeters: 0.508 @@ -302,9 +302,9 @@ subsystems: distanceThreeInclineAngleRotations: 0.07425 distanceThreeLaunchVelocityRPS: 49 distanceAutoThingLaunchVelocityRPS: 60 - closeDistanceBetweenBeamBreaks: 0.419 - farDistanceBetweenBeamBreaks: 13.570 - secondLowestBeamBreakToZero: 6.7769 + closeDistanceBetweenBeamBreaks: 0.026816 + farDistanceBetweenBeamBreaks: 0.86848 + secondLowestBeamBreakToZero: 0.4337216 turretRotationToleranceDegrees: 15 inclineAngleToleranceDegrees: 2 launcherVelocityToleranceRPS: 2 From 20d249deadac40f095dee755538ba24a1c7b3d21 Mon Sep 17 00:00:00 2001 From: henrymilbert <110063972+henrymilbert@users.noreply.github.com> Date: Sat, 28 Mar 2026 14:13:03 -0500 Subject: [PATCH 2/4] add auto path preview and clean up auto pose initialization --- src/main/java/com/team1816/lib/BaseRobot.java | 11 + .../com/team1816/lib/BaseRobotContainer.java | 50 +--- .../java/com/team1816/lib/BaseRobotState.java | 5 + .../team1816/lib/auto/AutoModeManager.java | 228 +++++++++++++++--- src/main/java/com/team1816/season/Robot.java | 13 +- 5 files changed, 213 insertions(+), 94 deletions(-) diff --git a/src/main/java/com/team1816/lib/BaseRobot.java b/src/main/java/com/team1816/lib/BaseRobot.java index 20ab10ea..70f9090e 100644 --- a/src/main/java/com/team1816/lib/BaseRobot.java +++ b/src/main/java/com/team1816/lib/BaseRobot.java @@ -41,6 +41,17 @@ protected BaseRobot() { public void robotPeriodic() { // Update the pose of the robot on the field. FieldContainer.field.setRobotPose(BaseRobotState.robotPose); + // Update the path preview until the match starts. + if (!BaseRobotState.hasAutoStarted) { + baseRobotContainer.autoModeManager.updateAutoPathPreviewDisplay(); + } + } + + @Override + public void autonomousInit() { + BaseRobotState.hasAutoStarted = true; + // Clear the path preview at the start of the match. + baseRobotContainer.autoModeManager.clearAutoPathPreviewDisplay(); } /** diff --git a/src/main/java/com/team1816/lib/BaseRobotContainer.java b/src/main/java/com/team1816/lib/BaseRobotContainer.java index 54e1c8a6..5d89b2a4 100644 --- a/src/main/java/com/team1816/lib/BaseRobotContainer.java +++ b/src/main/java/com/team1816/lib/BaseRobotContainer.java @@ -1,7 +1,5 @@ package com.team1816.lib; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.util.FlippingUtil; import com.team1816.lib.auto.AutoModeManager; import com.team1816.lib.auto.PathfindManager; import com.team1816.lib.inputs.CommandButtonBoard; @@ -9,10 +7,6 @@ import com.team1816.lib.subsystems.LedManager; import com.team1816.lib.subsystems.Vision; import com.team1816.lib.subsystems.drivetrain.Swerve; -import com.team1816.lib.util.GreenLogger; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public abstract class BaseRobotContainer { @@ -23,7 +17,6 @@ public abstract class BaseRobotContainer { protected final Swerve swerve = new Swerve(driverController); protected final Vision vision = new Vision(); - private boolean poseInitialized; protected PathfindManager pathfindManager; public AutoModeManager autoModeManager; @@ -36,48 +29,7 @@ public void initializeLibSubSystems() { Singleton.CreateSubSystem(LedManager.class); pathfindManager = Singleton.get(PathfindManager.class); - autoModeManager = Singleton.get(AutoModeManager.class); - - if (autoModeManager != null) { - autoModeManager.onChange(this::updatePoseOnSelection); - } else { - GreenLogger.log("Failed to initialize AutoModeManager!"); - } - } - - public void updateInitialPose(){ - if(poseInitialized || DriverStation.getAlliance().isEmpty()) return; - forceUpdatePose(); - } - - /** - * Forces pose update regardless of poseInitialized state. - * Called from autonomousInit to ensure pose is always set before auto starts. - */ - public void forceUpdatePose(){ - updatePoseOnSelection(autoModeManager.getSelectedAuto()); - } - - private void updatePoseOnSelection(Command selectedAuto) { - if (selectedAuto != null) { - try { - Pose2d startingPose = selectedAuto instanceof PathPlannerAuto - ? ((PathPlannerAuto) selectedAuto).getStartingPose() - : Pose2d.kZero; - if (startingPose != null) { - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { - startingPose = FlippingUtil.flipFieldPose(startingPose); - } - // Reset odometry and update Field2d this is to give drivers clue that the - // proper auto is set prior to auto start - swerve.resetPose(startingPose); - poseInitialized = true; - } - } catch (Exception e) { - GreenLogger.log("Error loading auto pose: " + e.getMessage()); - } - } + autoModeManager = new AutoModeManager(swerve::resetPose); } /** diff --git a/src/main/java/com/team1816/lib/BaseRobotState.java b/src/main/java/com/team1816/lib/BaseRobotState.java index 96430a90..4b331d93 100644 --- a/src/main/java/com/team1816/lib/BaseRobotState.java +++ b/src/main/java/com/team1816/lib/BaseRobotState.java @@ -43,4 +43,9 @@ public class BaseRobotState { * to be the pre-vision position estimate of the real robot. */ public static Pose2d simActualOrRawOdometryPose = Pose2d.kZero; + + /** + * If autonomous has ever been initialized. + */ + public static volatile boolean hasAutoStarted = false; } diff --git a/src/main/java/com/team1816/lib/auto/AutoModeManager.java b/src/main/java/com/team1816/lib/auto/AutoModeManager.java index 716a2024..a3646f2c 100644 --- a/src/main/java/com/team1816/lib/auto/AutoModeManager.java +++ b/src/main/java/com/team1816/lib/auto/AutoModeManager.java @@ -2,83 +2,245 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.FlippingUtil; -import com.pathplanner.lib.util.PPLibTelemetry; +import com.pathplanner.lib.util.PathPlannerLogging; +import com.team1816.lib.BaseRobotState; +import com.team1816.lib.util.FieldContainer; import com.team1816.lib.util.GreenLogger; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import org.json.simple.parser.ParseException; -import java.util.Optional; +import java.io.IOException; +import java.util.*; import java.util.function.Consumer; import java.util.stream.Stream; /** - * Manages SmartDashboard and logging of autos. Does NOT interact directly with {@link CommandScheduler} or the drivetrain. + * Manages creating the auto chooser to select the auto routine. Also handles resetting the robot's + * pose based on the auto starting pose, and drawing the path preview. Does NOT interact directly + * with {@link CommandScheduler} or the drivetrain. */ public class AutoModeManager { private final SendableChooser autoChooser; - private Consumer listener; + /** + * A mapping of the auto names as displayed in the {@link #autoChooser} to the path preview for + * that auto to display on the dashboard. + */ + private final HashMap> autoPathPreviewMap = new HashMap<>(); + /** + * A consumer that will accept a {@link Pose2d} to reset the robot's pose to. + */ + private final Consumer resetPoseConsumer; /** - * Puts dropdown in SmartDashboard and adds all autos. + * Constructs the {@link AutoModeManager}, including putting the auto dropdown in + * SmartDashboard and handling the creation of mirrored versions of autos. + * + * @param resetPoseConsumer A consumer that will accept a {@link Pose2d} that is the auto + * starting pose to reset the robot's pose to. */ - public AutoModeManager() { + public AutoModeManager(Consumer resetPoseConsumer) { + this.resetPoseConsumer = resetPoseConsumer; + + // Build the auto chooser with all the autos from PathPlanner and mirrored versions of most + // autos. autoChooser = AutoBuilder.buildAutoChooserWithOptionsModifier( autos -> autos.flatMap(auto -> { // Get the name of the original auto for PathPlanner to look for. - String autoName = auto.getName(); + String originalAutoName = auto.getName(); // Don't create a mirrored version if marked with "[DM]" (Don't Mirror). - if (autoName.startsWith("[DM]")) { - // Remove the "[DM]" marker for the display name, and any spaces. - auto.setName(autoName.substring(5).stripLeading()); + if (originalAutoName.startsWith("[DM]")) { + // Remove the "[DM]" marker for the display name, and any leading spaces. + String modifiedAutoName = originalAutoName.substring(5).stripLeading(); + auto.setName(modifiedAutoName); + addAutoPathPreviewToMap(originalAutoName, modifiedAutoName, false); return Stream.of(auto); } // Rename the original auto to specify it is the left version. - auto.setName("Left " + autoName); + String leftAutoName = "Left " + originalAutoName; + auto.setName(leftAutoName); + addAutoPathPreviewToMap(originalAutoName, leftAutoName, false); // Create a mirrored version of the auto. - PathPlannerAuto mirroredAuto = new PathPlannerAuto(autoName, true); + PathPlannerAuto mirroredAuto = new PathPlannerAuto(originalAutoName, true); // Rename the mirrored auto to specify it is the right version. - mirroredAuto.setName("Right " + autoName); + String rightAutoName = "Right " + originalAutoName; + mirroredAuto.setName(rightAutoName); + addAutoPathPreviewToMap(originalAutoName, rightAutoName, true); // Return both the original and the mirrored auto. return Stream.of(auto, mirroredAuto); }) ); + + // Publish the auto chooser to the SmartDashboard. SmartDashboard.putData("Auto Mode", autoChooser); - listener = a -> {}; + // Display the active path on the main Field2d. This is the path that PathPlanner is + // actively running, not a path that is scheduled to run once auto starts. + PathPlannerLogging.setLogActivePathCallback(pose2ds -> + FieldContainer.field.getObject("activePath").setPoses(pose2ds) + ); + + // Create a trigger and use it to reset the pose to the auto start pose whenever the + // alliance color changes. + new Trigger(() -> + // If the alliance is blue, defaulting to true if the alliance is empty. + DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Blue + ) + .onChange(Commands.runOnce(() -> + resetRobotPoseToAutoStart(autoChooser.getSelected(), false) + ).ignoringDisable(true)); + + // Register a listener to reset the pose to the auto start pose whenever the selected auto + // changes. + autoChooser.onChange(autoCommand -> resetRobotPoseToAutoStart(autoCommand, false)); - autoChooser.onChange(a -> { - // log starting pose - Pose2d startingPose = a instanceof PathPlannerAuto - ? ((PathPlannerAuto) a).getStartingPose() + // Use the autonomous trigger to force reset the pose to the auto start pose whenever auto + // starts, to make sure the pose is set correctly at the beginning of auto. + RobotModeTriggers.autonomous().onTrue(Commands.runOnce(() -> + resetRobotPoseToAutoStart(autoChooser.getSelected(), true) + ).ignoringDisable(true)); + } + + /** + * Resets the robot pose using the {@link #resetPoseConsumer} to the start pose of the passed + * in auto command. + * + * @param autoCommand The auto command to use the start pose of. + * @param forceResetPose If the pose should be reset regardless of whether the match has + * already started. + */ + private void resetRobotPoseToAutoStart(Command autoCommand, boolean forceResetPose) { + // Only reset the pose if auto has not started, or if forceResetPose is true. This is to + // prevent resetting the pose in the middle of a match. This is based on if auto has ever + // started, rather than just checking if we are currently in teleop or auto, because we + // want to prevent the pose resetting even during the disabled period between auto and + // teleop. + if (!BaseRobotState.hasAutoStarted || forceResetPose) { + // Get the starting pose for the auto. If this is the default None auto, it will just + // be an empty Command rather than a PathPlannerAuto, so use Pose2d.kZero in that case. + Pose2d startingPose = autoCommand instanceof PathPlannerAuto + ? ((PathPlannerAuto) autoCommand).getStartingPose() : Pose2d.kZero; - Optional alliance = DriverStation.getAlliance(); - if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { - startingPose = FlippingUtil.flipFieldPose(startingPose); - } - GreenLogger.log("Auto Mode Manager- starting pose: " + startingPose); - // TODO: why is this not PPLibTelemetry.setCurrentPose? look into this after Pittsburgh - PPLibTelemetry.setTargetPose(startingPose); + // Flip the starting pose based on alliance, defaulting to blue if the alliance is empty. + startingPose = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Blue + ? startingPose + : FlippingUtil.flipFieldPose(startingPose); + // Call the consumer to reset the robot's pose. + resetPoseConsumer.accept(startingPose); + } + } + + /** + * Updates the display of the selected auto path on the field. + */ + public void updateAutoPathPreviewDisplay() { + String autoName = autoChooser.getSelected().getName(); + List path = autoPathPreviewMap.get(autoName); + if (path != null) { + // Flip the path based on alliance, defaulting to blue if the alliance is empty. + path = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Blue + ? path + : path.stream().map(FlippingUtil::flipFieldPose).toList(); + // The number of Pose2ds to move through in the path per second. + double pose2dsPerSecond = 300; + // Zero-indexed progress through the path to display. Use the timer to have it count + // up, using modulus to loop back to the start when the path ends. + int pathProgress = (int) ((Timer.getFPGATimestamp() * pose2dsPerSecond) % (path.size())); + // Show the path on the field up to the pathProgress. + FieldContainer.field.getObject("autoPathPreview").setPoses( + path.stream().limit(pathProgress + 1).toList() + ); + } + else { + // Clear the poses if no valid path was found. + clearAutoPathPreviewDisplay(); + } + } - if (listener != null) listener.accept(a); - }); + /** + * Clear the display of the selected auto path on the field. + */ + public void clearAutoPathPreviewDisplay() { + FieldContainer.field.getObject("autoPathPreview").setPoses(); } /** - * Retrieves currently selected auto. + * Adds the paths from the auto to the {@link #autoPathPreviewMap} so we can look it up later for + * displaying on the field using the name stored in the {@link #autoChooser}. This is necessary + * because we modify the auto names when we build the auto chooser, and the only way to get the + * paths is to load the auto from the file, which requires having the original auto file name. + * + * @param originalAutoName The name of the auto file that contains the paths. + * @param modifiedAutoName The modified name of the auto that will be used later to find the + * paths from the map. + * @param mirror If the paths stored should be mirrored to the other side of the current + * alliance from the original paths stored in the auto file. */ - public Command getSelectedAuto() { - return autoChooser.getSelected(); + private void addAutoPathPreviewToMap(String originalAutoName, String modifiedAutoName, boolean mirror) { + try { + // Load the paths for the auto. + List pathPlannerPaths = PathPlannerAuto.getPathGroupFromAutoFile(originalAutoName); + + // Combine the paths into a single list of Pose2ds, with repeated Pose2ds at the end of + // each path and at the very end to create pauses when running through the path with + // the preview. + // The number of times to repeat the last Pose2d in each path. + int pathEndRepeats = 5; + // The number of times to repeat the last Pose2d in the whole auto. + int autoEndRepeats = 10; + List combinedPath = new ArrayList<>(); + for (int i = 0; i < pathPlannerPaths.size(); i++) { + PathPlannerPath path = pathPlannerPaths.get(i); + + // Mirror the path if necessary. + if (mirror) path = path.mirrorPath(); + + // Get the list of poses representing the path. + List pathPoses = path.getPathPoses(); + if (pathPoses.isEmpty()) continue; + + // Add all the poses from the individual path to the combined path. + combinedPath.addAll(pathPoses); + + // Repeat the last pose in the individual path to create a pause at the end of each + // path. + Pose2d lastPose = pathPoses.get(pathPoses.size() - 1); + combinedPath.addAll(Collections.nCopies(pathEndRepeats, lastPose)); + + // If this is the last path in the auto, repeat the last pose in the path to create + // a longer pause at the very end of the auto. + if (i == pathPlannerPaths.size() - 1) { + combinedPath.addAll(Collections.nCopies(autoEndRepeats, lastPose)); + } + } + + autoPathPreviewMap.put( + modifiedAutoName, + combinedPath + ); + } catch (IOException e) { + GreenLogger.log(e); + GreenLogger.log("IOException while getting path group. See above stack trace for details."); + } catch (ParseException e) { + GreenLogger.log(e); + GreenLogger.log("ParseException while getting path group. See above stack trace for details."); + } } /** - * Registers a single listener to changes to the auto choosers. + * Retrieves currently selected auto from the {@link #autoChooser}. */ - public void onChange(Consumer listener) { - this.listener = listener; + public Command getSelectedAuto() { + return autoChooser.getSelected(); } } diff --git a/src/main/java/com/team1816/season/Robot.java b/src/main/java/com/team1816/season/Robot.java index 71c961fb..31161ee9 100644 --- a/src/main/java/com/team1816/season/Robot.java +++ b/src/main/java/com/team1816/season/Robot.java @@ -53,22 +53,11 @@ public void disabledInit() { } } - @Override - public void disabledPeriodic() { - try { - robotContainer.updateInitialPose(); - } catch (Throwable t) { - robotStatusEvent.Publish(LedManager.RobotLEDStatus.ERROR); - GreenLogger.log(t); - } - } - @Override public void autonomousInit() { try { + super.autonomousInit(); robotContainer.autonomousInit(); - // Ensure pose is always initialized before scheduling auto - robotContainer.forceUpdatePose(); autonomousCommand = robotContainer.autoModeManager.getSelectedAuto(); // schedule the autonomous command if (autonomousCommand != null) { From 1af6f150d114525a7526b7fd5578eed8efdd567a Mon Sep 17 00:00:00 2001 From: henrymilbert <110063972+henrymilbert@users.noreply.github.com> Date: Sun, 29 Mar 2026 13:11:05 -0500 Subject: [PATCH 3/4] commit simgui changes for path preview and active path --- simgui.json | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/simgui.json b/simgui.json index 9e335a29..79983658 100644 --- a/simgui.json +++ b/simgui.json @@ -52,6 +52,14 @@ ], "style": "Hidden" }, + "activePath": { + "arrows": false, + "style": "Line" + }, + "autoPathPreview": { + "arrows": false, + "style": "Line" + }, "bottom": 1914, "height": 8.069275856018066, "left": 245, From 344093986e0913844f11ccdb3f297c3aed6bd1ce Mon Sep 17 00:00:00 2001 From: henrymilbert <110063972+henrymilbert@users.noreply.github.com> Date: Wed, 1 Apr 2026 18:27:51 -0500 Subject: [PATCH 4/4] scale up turret gains to match sensorToMechanismRatio change --- src/main/resources/yaml/ztldr.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/resources/yaml/ztldr.yml b/src/main/resources/yaml/ztldr.yml index b01b3a49..df5e4a5b 100644 --- a/src/main/resources/yaml/ztldr.yml +++ b/src/main/resources/yaml/ztldr.yml @@ -273,12 +273,12 @@ subsystems: pidConfig: slot0: kS: 0.1 # static friction - increase if turret won't move at small errors - kP: 4 # position error gain - increase if still lagging, decrease if oscillating at setpoint - kD: 0.28 # damping - increase if oscillating, keep roughly kP/20 + kP: 62.5 # position error gain - increase if still lagging, decrease if oscillating at setpoint + kD: 4.375 # damping - increase if oscillating, keep roughly kP/20 staticFeedforwardSign: UseClosedLoopSign motionMagic: - expoKV: 0.04 #0.10 # cruise speed - lower = faster moves, higher = slower. Start here, decrease if turret feels sluggish - expoKA: 0.1 #0.2 # acceleration smoothness - increase if dead zone reversals are too violent, decrease if turret feels mushy + expoKV: 0.625 #0.10 # cruise speed - lower = faster moves, higher = slower. Start here, decrease if turret feels sluggish + expoKA: 1.5625 #0.2 # acceleration smoothness - increase if dead zone reversals are too violent, decrease if turret feels mushy inclineCoder: id: 29 deviceType: CANcoder