diff --git a/simgui.json b/simgui.json index 7a24c876..f9b43a73 100644 --- a/simgui.json +++ b/simgui.json @@ -13,9 +13,9 @@ "/SmartDashboard/IMU": "Alerts", "/SmartDashboard/JSON": "Alerts", "/SmartDashboard/Motors": "Alerts", - "/SmartDashboard/PathPlanner": "Alerts", "/SmartDashboard/Scheduler": "Scheduler", - "/SmartDashboard/Swerve Drive": "Alerts" + "/SmartDashboard/Swerve Drive": "Alerts", + "/SmartDashboard/VisionSystemSim-Vision/Sim Field": "Field2d" }, "windows": { "/SmartDashboard/Field": { @@ -51,7 +51,6 @@ "width": 0.30000001192092896 }, "bottom": 1638, - "builtin": "2025 Reefscape", "height": 8.051901817321777, "left": 534, "right": 3466, @@ -71,8 +70,8 @@ "open": true } }, - "Auto Controllers": { - "X": { + "Controllers": { + "Xbox": { "open": true }, "open": true @@ -87,8 +86,7 @@ "swerve": { "modules": { "open": true - }, - "open": true + } } } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ccf67b13..aee7f7a7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -49,7 +49,7 @@ public static final class IOConstants { // When test mode is enabled, the operator controller is used for driving and testing // This should always be false on the main branch - public static final boolean kTestMode = false; + public static final boolean kTestMode = Robot.isSimulation(); } public static final class Vision { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 694b0b4d..3a981d7a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -54,6 +54,10 @@ public void robotInit() { DriverStation.silenceJoystickConnectionWarning(true); SimulatedArena.getInstance().resetFieldForAuto(); } + + if (Constants.IOConstants.kTestMode) { + System.out.println("Test Mode Enabled\nNot for competition use"); + } } public static Robot getInstance() { @@ -80,14 +84,10 @@ public void robotPeriodic() { SmartDashboard.putData(CommandScheduler.getInstance()); if (Constants.IOConstants.kTestMode) { - System.out.println("Test Mode Enabled\nNot for competition use"); + // System.out.println("Test Mode Enabled\nNot for competition use"); } } - /** This function is called once each time the robot enters Disabled mode. */ - @Override - public void disabledInit() {} - @Override public void disabledPeriodic() { if (m_autonomousCommand != null) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5622f4e0..edbac5b7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,7 @@ import choreo.auto.AutoChooser; import choreo.auto.AutoFactory; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -16,6 +17,7 @@ import frc.robot.commands.GrabCoralCommand; import frc.robot.commands.ReleaseCoralCommand; import frc.robot.commands.drive.Drive; +import frc.robot.commands.drive.MoveToPose; import frc.robot.subsystems.ChuteSubsystem; import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.CoralHandlerSubsystem; @@ -23,10 +25,12 @@ import frc.robot.subsystems.sim.CoralHandlerSubsystemSim; import frc.robot.subsystems.sim.ElevatorSubsystemSim; import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.AprilTags; import frc.robot.utils.FilteredButton; import frc.robot.utils.FilteredJoystick; import java.io.File; import java.util.Optional; +import java.util.Set; /* * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -75,13 +79,18 @@ public RobotContainer() { m_autoChooser.addRoutine("Blue Reef K Routine", m_routines::blueCoralToReefK); m_autoChooser.addRoutine("Blue Test Full Routine", m_routines::blueTestFull); SmartDashboard.putData("Auto Chooser", m_autoChooser); + SmartDashboard.putData("Xbox Controller Debug", m_controller.getHID()); + + if (Robot.isSimulation()) { + DriverStation.silenceJoystickConnectionWarning(true); + } if (IOConstants.kTestMode) { m_drive.setDefaultCommand( new Drive( m_drive, - m_controller::getLeftY, - m_controller::getLeftX, + () -> -m_controller.getLeftY(), + () -> -m_controller.getLeftX(), () -> -m_controller.getRightX(), () -> m_controller.rightBumper().getAsBoolean(), Optional.empty())); @@ -121,10 +130,10 @@ private void configureButtonBindings() { // Test mode allows everything to be run on a single controller // Test mode should not be enabled in competition + if (IOConstants.kTestMode) { m_controller.a().onTrue(Commands.runOnce(m_drive::zeroGyro)); } else { - // drop chute m_buttons.getChuteSwitch().onTrue(Commands.runOnce(m_chute::drop)); @@ -140,5 +149,36 @@ private void configureButtonBindings() { m_controller.rightBumper().onTrue(new ReleaseCoralCommand(m_coral)); m_controller.leftBumper().onTrue(new GrabCoralCommand(m_coral)); } + + m_controller + .rightStick() + .whileTrue( + Commands.defer( + () -> + MoveToPose.tagRelative( + this.m_drive, + AprilTags.findReefTagForAlignment(this.m_drive.getPose()), + AprilTags.REEF_ALIGN_OFFSET) + .withDriveInputs( + m_controller::getLeftX, + m_controller::getLeftY, + () -> -m_controller.getRightX(), + 0.5), + Set.of(this.m_drive))); + m_controller + .leftStick() + .whileTrue( + Commands.defer( + () -> + MoveToPose.tagRelative( + this.m_drive, + AprilTags.findStationTagForAlignment(this.m_drive.getPose()), + AprilTags.STATION_ALIGN_OFFSET) + .withDriveInputs( + m_controller::getLeftX, + m_controller::getLeftY, + () -> -m_controller.getRightX(), + 0.5), + Set.of(this.m_drive))); } } diff --git a/src/main/java/frc/robot/commands/drive/MoveToPose.java b/src/main/java/frc/robot/commands/drive/MoveToPose.java new file mode 100644 index 00000000..2a89648e --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/MoveToPose.java @@ -0,0 +1,226 @@ +package frc.robot.commands.drive; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.subsystems.swervedrive.Vision; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class MoveToPose extends Command { + + private final SwerveSubsystem drive; + private final Supplier pose; + + private Supplier linearFF = () -> Translation2d.kZero; + private DoubleSupplier omegaFF = () -> 0.0; + + private final ProfiledPIDController driveController = + new ProfiledPIDController(0.8, 0., 0., new TrapezoidProfile.Constraints(3.8, 3.)); + private final ProfiledPIDController thetaController = + new ProfiledPIDController(4., 0., 0., new TrapezoidProfile.Constraints(2. * Math.PI, 8.)); + private final double ffMinRadius = 0.05; + private final double ffMaxRadius = 0.1; + + private Translation2d lastSetpointTranslation = Translation2d.kZero; + private double driveErrorAbs = 0.0; + private double thetaErrorAbs = 0.0; + + private Translation2d lastLinearInput = Translation2d.kZero; + private double lastRotInput = 0.0; + private final double inputSmoothFactor = 0.4; // Lower = smoother (0.0-1.0) + private final double maxLinearDeviation = 0.4; // Maximum influence as fraction of max speed + private final double maxRotDeviation = 0.5; // Maximum influence as fraction of max rot speed + + private final FieldObject2d debugPos; + + { + thetaController.enableContinuousInput(-Math.PI, Math.PI); + } + + public MoveToPose(SwerveSubsystem drive, Supplier pose) { + this.drive = drive; + this.pose = pose; + this.debugPos = this.drive.getSwerveDrive().field.getObject("Auto Alignment/Desired"); + } + + public static MoveToPose tagRelative(SwerveSubsystem drive, int tagId, Transform2d transform) { + return new MoveToPose(drive, () -> Vision.getAprilTagPose(tagId, transform)); + } + + @Override + public void initialize() { + final var currentPose = this.drive.getPose(); + final var targetPose = this.pose.get(); + final var fieldVelocity = this.drive.getFieldVelocity(); + + Translation2d linearFieldVelocity = + new Translation2d(fieldVelocity.vxMetersPerSecond, fieldVelocity.vyMetersPerSecond); + driveController.reset( + currentPose.getTranslation().getDistance(targetPose.getTranslation()), + Math.min( + 0.0, + -linearFieldVelocity + .rotateBy( + targetPose + .getTranslation() + .minus(currentPose.getTranslation()) + .getAngle() + .unaryMinus()) + .getX())); + thetaController.reset( + currentPose.getRotation().getRadians(), fieldVelocity.omegaRadiansPerSecond); + lastSetpointTranslation = currentPose.getTranslation(); + } + + @Override + public void execute() { + final var currentPose = this.drive.getPose(); + final var targetPose = this.pose.get(); + this.debugPos.setPose(targetPose); + + // https://github.com/Mechanical-Advantage/RobotCode2025Public/blob/main/src/main/java/org/littletonrobotics/frc2025/commands/DriveToPose.java + // Calculate drive speed + double currentDistance = currentPose.getTranslation().getDistance(targetPose.getTranslation()); + double ffScaler = + MathUtil.clamp((currentDistance - ffMinRadius) / (ffMaxRadius - ffMinRadius), 0.0, 1.0); + driveErrorAbs = currentDistance; + driveController.reset( + lastSetpointTranslation.getDistance(targetPose.getTranslation()), + driveController.getSetpoint().velocity); + double driveVelocityScalar = + driveController.getSetpoint().velocity * ffScaler + + driveController.calculate(driveErrorAbs, 0.0); + if (currentDistance < driveController.getPositionTolerance()) driveVelocityScalar = 0.0; + lastSetpointTranslation = + new Pose2d( + targetPose.getTranslation(), + new Rotation2d( + Math.atan2( + currentPose.getTranslation().getY() - targetPose.getTranslation().getY(), + currentPose.getTranslation().getX() - targetPose.getTranslation().getX()))) + .transformBy( + new Transform2d(driveController.getSetpoint().position, 0.0, Rotation2d.kZero)) + .getTranslation(); + + // Calculate theta speed + double thetaVelocity = + thetaController.getSetpoint().velocity * ffScaler + + thetaController.calculate( + currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians()); + thetaErrorAbs = + Math.abs(currentPose.getRotation().minus(targetPose.getRotation()).getRadians()); + if (thetaErrorAbs < thetaController.getPositionTolerance()) thetaVelocity = 0.0; + + Translation2d driveVelocity = + new Pose2d( + Translation2d.kZero, + new Rotation2d( + Math.atan2( + currentPose.getTranslation().getY() - targetPose.getTranslation().getY(), + currentPose.getTranslation().getX() - targetPose.getTranslation().getX()))) + .transformBy(new Transform2d(driveVelocityScalar, 0.0, Rotation2d.kZero)) + .getTranslation(); + + // Scale feedback velocities by input ff + final double linearS = linearFF.get().getNorm() * 3.0; + final double thetaS = Math.abs(omegaFF.getAsDouble()) * 3.0; + driveVelocity = + driveVelocity.interpolate( + linearFF.get().times(Constants.DriveConstants.kMaxSpeedMetersPerSecond), linearS); + thetaVelocity = + MathUtil.interpolate( + thetaVelocity, + omegaFF.getAsDouble() * Constants.DriveConstants.kMaxAngularSpeed, + thetaS); + + // Command speeds + drive.drive( + ChassisSpeeds.fromFieldRelativeSpeeds( + driveVelocity.getX(), driveVelocity.getY(), thetaVelocity, currentPose.getRotation())); + } + + /** + * Adds controller inputs to the autonomous movement, allowing manual adjustments while the + * command is running with smoothing and maximum deviation limits. + * + * @param xInput X axis input supplier (forward/backward) + * @param yInput Y axis input supplier (left/right) + * @param rotInput Rotational input supplier + * @param scale Scale factor for inputs (0.0-1.0) + * @return this command object for method chaining + */ + public MoveToPose withDriveInputs( + DoubleSupplier xInput, DoubleSupplier yInput, DoubleSupplier rotInput, double scale) { + this.linearFF = + () -> { + // Apply deadband and get raw inputs + double x = MathUtil.applyDeadband(xInput.getAsDouble(), 0.1); + double y = MathUtil.applyDeadband(yInput.getAsDouble(), 0.1); + Translation2d rawInput = new Translation2d(x, y); + + // Limit maximum magnitude + if (rawInput.getNorm() > 1.0) { + rawInput = rawInput.times(1.0 / rawInput.getNorm()); + } + + // Apply user scale factor + rawInput = rawInput.times(scale); + + // Apply maximum deviation limit + rawInput = rawInput.times(maxLinearDeviation); + + // Smooth the input + Translation2d smoothedInput = + new Translation2d( + MathUtil.interpolate(lastLinearInput.getX(), rawInput.getX(), inputSmoothFactor), + MathUtil.interpolate(lastLinearInput.getY(), rawInput.getY(), inputSmoothFactor)); + + // Save for next cycle + lastLinearInput = smoothedInput; + + return smoothedInput; + }; + + this.omegaFF = + () -> { + // Apply deadband and get raw input + double rotVal = MathUtil.applyDeadband(rotInput.getAsDouble(), 0.1); + + // Apply user scale factor + rotVal *= scale; + + // Apply maximum deviation limit + rotVal *= maxRotDeviation; + + // Smooth the input + double smoothedRotVal = MathUtil.interpolate(lastRotInput, rotVal, inputSmoothFactor); + + // Save for next cycle + lastRotInput = smoothedRotVal; + + return smoothedRotVal; + }; + + return this; + } + + @Override + public boolean isFinished() { + return this.driveController.atGoal() && this.thetaController.atGoal(); + } + + @Override + public void end(boolean interrupted) { + this.debugPos.setPoses(); + } +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index c013cede..dc6344b1 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -20,7 +20,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.Constants; -import frc.robot.subsystems.swervedrive.Vision.Cameras; +import frc.robot.subsystems.swervedrive.Vision.Camera; import java.io.File; import java.util.Arrays; import java.util.Optional; @@ -49,7 +49,7 @@ public class SwerveSubsystem extends SubsystemBase { // private final AprilTagFieldLayout aprilTagFieldLayout = // AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); /** Enable vision odometry updates while driving. */ - private final boolean visionDriveTest = false; + private final boolean visionDriveTest = true; /** PhotonVision class to keep an accurate odometry. */ private Vision vision; @@ -168,6 +168,10 @@ public void setupPhotonVision() { vision = new Vision(swerveDrive::getPose, swerveDrive.field); } + public Vision getVision() { + return vision; + } + @Override public void periodic() { // When vision is enabled we must manually update odometry in SwerveDrive @@ -187,7 +191,7 @@ public void simulationPeriodic() {} * * @return A {@link Command} which will run the alignment. */ - public Command aimAtTarget(Cameras camera) { + public Command aimAtTarget(Camera camera) { return run( () -> { diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index f1baba1a..5888e962 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -81,7 +81,7 @@ public Vision(Supplier currentPose, Field2d field) { visionSim = new VisionSystemSim("Vision"); visionSim.addAprilTags(fieldLayout); - for (Cameras c : Cameras.values()) { + for (Camera c : Camera.values()) { c.addToVisionSim(visionSim); } @@ -92,18 +92,18 @@ public Vision(Supplier currentPose, Field2d field) { /** * Calculates a target pose relative to an AprilTag on the field. * - * @param aprilTag The ID of the AprilTag. + * @param aprilTagID The ID of the AprilTag. * @param robotOffset The offset {@link Transform2d} of the robot to apply to the pose for the * robot to position itself correctly. * @return The target pose of the AprilTag. */ - public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) { - Optional aprilTagPose3d = fieldLayout.getTagPose(aprilTag); + public static Pose2d getAprilTagPose(int aprilTagID, Transform2d robotOffset) { + Optional aprilTagPose3d = fieldLayout.getTagPose(aprilTagID); if (aprilTagPose3d.isPresent()) { return aprilTagPose3d.get().toPose2d().transformBy(robotOffset); } else { throw new RuntimeException( - "Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString()); + "Cannot get AprilTag " + aprilTagID + " from field " + fieldLayout.toString()); } } @@ -124,7 +124,7 @@ public void updatePoseEstimation(SwerveDrive swerveDrive) { */ visionSim.update(swerveDrive.getSimulationDriveTrainPose().get()); } - for (Cameras camera : Cameras.values()) { + for (Camera camera : Camera.values()) { Optional poseEst = getEstimatedGlobalPose(camera); if (poseEst.isPresent()) { var pose = poseEst.get(); @@ -145,7 +145,7 @@ public void updatePoseEstimation(SwerveDrive swerveDrive) { * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to * create the estimate */ - public Optional getEstimatedGlobalPose(Cameras camera) { + public Optional getEstimatedGlobalPose(Camera camera) { Optional poseEst = camera.getEstimatedGlobalPose(); if (Robot.isSimulation()) { Field2d debugField = visionSim.getDebugField(); @@ -178,7 +178,7 @@ public double getDistanceFromAprilTag(int id) { * @param camera Camera to check. * @return Tracked target. */ - public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) { + public PhotonTrackedTarget getTargetFromId(int id, Camera camera) { PhotonTrackedTarget target = null; for (PhotonPipelineResult result : camera.resultsList) { if (result.hasTargets()) { @@ -223,7 +223,7 @@ private void openSimCameraViews() { public void updateVisionField() { List targets = new ArrayList(); - for (Cameras c : Cameras.values()) { + for (Camera c : Camera.values()) { if (!c.resultsList.isEmpty()) { PhotonPipelineResult latest = c.resultsList.get(0); if (latest.hasTargets()) { @@ -244,7 +244,7 @@ public void updateVisionField() { } /** Camera Enum to select each camera */ - enum Cameras { + public enum Camera { /** Left Camera */ LEFT_CAM( "left", @@ -319,7 +319,7 @@ enum Cameras { * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated poses from the * camera. */ - Cameras( + Camera( String name, Rotation3d robotToCamRotation, Translation3d robotToCamTranslation, @@ -448,7 +448,7 @@ private void updateUnreadResults() { * only be called once per loop. * *

Also includes updates for the standard deviations, which can (optionally) be retrieved - * with {@link Cameras#updateEstimationStdDevs} + * with {@link Camera#updateEstimationStdDevs} * * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets * used for estimation. diff --git a/src/main/java/frc/robot/utils/AprilTags.java b/src/main/java/frc/robot/utils/AprilTags.java new file mode 100644 index 00000000..b1a050e0 --- /dev/null +++ b/src/main/java/frc/robot/utils/AprilTags.java @@ -0,0 +1,75 @@ +package frc.robot.utils; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.subsystems.swervedrive.Vision; +import java.util.List; + +public final class AprilTags { + + public static final List RED_REEF_TAGS = List.of(6, 7, 8, 9, 10, 11); + public static final List BLUE_REEF_TAGS = List.of(17, 18, 19, 20, 21, 22); + public static final List RED_STATION_TAGS = List.of(1, 2); + public static final List BLUE_STATION_TAGS = List.of(12, 13); + + private AprilTags() {} + + public static List reefTags() { + return DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Blue + ? BLUE_REEF_TAGS + : RED_REEF_TAGS; + } + + public static List stationTags() { + return DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Blue + ? BLUE_STATION_TAGS + : RED_STATION_TAGS; + } + + public static final Transform2d REEF_ALIGN_OFFSET = new Transform2d(0.6, 0, Rotation2d.kPi); + public static final Transform2d STATION_ALIGN_OFFSET = new Transform2d(0.6, 0, Rotation2d.kZero); + + public static int findReefTagForAlignment(Pose2d robotPose) { + var currentTag = -1; + var minDistance = Double.MAX_VALUE; + + for (final var tag : AprilTags.reefTags()) { + final var tagPose = Vision.getAprilTagPose(tag, REEF_ALIGN_OFFSET); + final var distance = robotPose.getTranslation().getDistance(tagPose.getTranslation()); + + if (distance < minDistance) { + currentTag = tag; + minDistance = distance; + } + } + + if (currentTag < 0) { + throw new RuntimeException("Expected to find a tag"); + } + + return currentTag; + } + + public static int findStationTagForAlignment(Pose2d robotPose) { + var currentTag = -1; + var minDistance = Double.MAX_VALUE; + + for (final var tag : AprilTags.stationTags()) { + final var tagPose = Vision.getAprilTagPose(tag, STATION_ALIGN_OFFSET); + final var distance = robotPose.getTranslation().getDistance(tagPose.getTranslation()); + + if (distance < minDistance) { + currentTag = tag; + minDistance = distance; + } + } + + if (currentTag < 0) { + throw new RuntimeException("Expected to find a tag"); + } + + return currentTag; + } +}