diff --git a/src/main/java/frc/robot/auto/Autos.java b/src/main/java/frc/robot/auto/Autos.java index c0c9345..0dd0a40 100644 --- a/src/main/java/frc/robot/auto/Autos.java +++ b/src/main/java/frc/robot/auto/Autos.java @@ -20,6 +20,7 @@ import frc.robot.subsystems.superstructure.ShotParameters; import frc.robot.subsystems.superstructure.Superstructure; import frc.robot.subsystems.vision.PhotonVision; +import org.littletonrobotics.junction.Logger; import java.util.List; import java.util.Set; @@ -146,7 +147,7 @@ private Command followPath(final ChoreoTrajectory choreoTrajectory, final Timer } private Command resetPose(final ChoreoTrajectory trajectory) { - return Commands.defer(() -> swerve.resetPoseCommand( + return Commands.defer(() -> photonVision.resetPoseCommand( Robot.IsRedAlliance.getAsBoolean() ? trajectory.getFlippedInitialPose() : trajectory.getInitialPose() @@ -1255,6 +1256,9 @@ public EventLoop driveAndNoteDetect() { autoTriggers.autoEnabled().whileTrue(preloadSubwooferAndFollow0(autoTriggers.trajectories, timer)); + Logger.recordOutput("OInitial pose", autoTriggers.trajectories.get(0).getInitialPose()); + Logger.recordOutput("Initial pose", autoTriggers.trajectories.get(0).getFlippedInitialPose()); + autoTriggers.atTime(2.23).onTrue( Commands.parallel( intake.intakeCommand().asProxy(), diff --git a/src/main/java/frc/robot/subsystems/drive/Swerve.java b/src/main/java/frc/robot/subsystems/drive/Swerve.java index 93dd6f0..21e9b93 100644 --- a/src/main/java/frc/robot/subsystems/drive/Swerve.java +++ b/src/main/java/frc/robot/subsystems/drive/Swerve.java @@ -404,14 +404,10 @@ public Command zeroRotationCommand() { return runOnce(this::zeroRotation); } - public void resetPose(final Pose2d robotPose) { + private void resetPose(final Pose2d robotPose) { poseEstimator.resetPosition(gyro.getYawRotation2d(), getModulePositions(), robotPose); } - public Command resetPoseCommand(final Pose2d robotPose) { - return runOnce(() -> resetPose(robotPose)); - } - public ChassisSpeeds getRobotRelativeSpeeds() { return kinematics.toChassisSpeeds( frontLeft.getState(), diff --git a/src/main/java/frc/robot/subsystems/drive/estimator/PoseEstimator.java b/src/main/java/frc/robot/subsystems/drive/estimator/PoseEstimator.java index 1fc1833..bbb63d4 100644 --- a/src/main/java/frc/robot/subsystems/drive/estimator/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/drive/estimator/PoseEstimator.java @@ -67,7 +67,8 @@ public PoseEstimator( Kinematics kinematics, Odometry odometry, Matrix stateStdDevs, - Matrix visionMeasurementStdDevs) { + Matrix visionMeasurementStdDevs + ) { m_kinematics = kinematics; m_odometry = odometry; @@ -175,7 +176,11 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS // Step 4: Convert back to Twist2d. var scaledTwist = - new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0)); + new Twist2d( + k_times_twist.get(0, 0), + k_times_twist.get(1, 0), + k_times_twist.get(2, 0) + ); // Step 5: Reset Odometry to state at sample with vision adjustment. m_odometry.resetPosition( @@ -226,7 +231,8 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS public void addVisionMeasurement( Pose2d visionRobotPoseMeters, double timestampSeconds, - Matrix visionMeasurementStdDevs) { + Matrix visionMeasurementStdDevs + ) { setVisionMeasurementStdDevs(visionMeasurementStdDevs); addVisionMeasurement(visionRobotPoseMeters, timestampSeconds); } @@ -256,7 +262,8 @@ public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, T m_odometry.update(gyroAngle, wheelPositions); m_poseBuffer.addSample( currentTimeSeconds, - new InterpolationRecord(getEstimatedPosition(), gyroAngle, wheelPositions.copy())); + new InterpolationRecord(getEstimatedPosition(), gyroAngle, wheelPositions.copy()) + ); return getEstimatedPosition(); } diff --git a/src/main/java/frc/robot/subsystems/drive/estimator/SwerveDrivePoseEstimator.java b/src/main/java/frc/robot/subsystems/drive/estimator/SwerveDrivePoseEstimator.java index 6909c3f..95433d7 100644 --- a/src/main/java/frc/robot/subsystems/drive/estimator/SwerveDrivePoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/drive/estimator/SwerveDrivePoseEstimator.java @@ -17,124 +17,129 @@ /** * This class wraps {@link SwerveDriveOdometry Swerve Drive Odometry} to fuse latency-compensated * vision measurements with swerve drive encoder distance measurements. It is intended to be a - * drop-in replacement for {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry}. + * drop-in replacement for {@link SwerveDriveOdometry}. * - *

{@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator#update} should be called every robot loop. + *

{@link SwerveDrivePoseEstimator#update} should be called every robot loop. * - *

{@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you + *

{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you * want; if you never call it, then this class will behave as regular encoder odometry. */ public class SwerveDrivePoseEstimator extends PoseEstimator { - private final int m_numModules; + private final int m_numModules; - /** - * Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision - * measurements. - * - *

The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y, - * and 0.1 radians for heading. The default standard deviations of the vision measurements are 0.9 - * meters for x, 0.9 meters for y, and 0.9 radians for heading. - * - * @param kinematics A correctly-configured kinematics object for your drivetrain. - * @param gyroAngle The current gyro angle. - * @param modulePositions The current distance measurements and rotations of the swerve modules. - * @param initialPoseMeters The starting pose estimate. - */ - public SwerveDrivePoseEstimator( - SwerveDriveKinematics kinematics, - Rotation2d gyroAngle, - SwerveModulePosition[] modulePositions, - Pose2d initialPoseMeters) { - this( - kinematics, - gyroAngle, - modulePositions, - initialPoseMeters, - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.9, 0.9, 0.9)); - } - - /** - * Constructs a SwerveDrivePoseEstimator. - * - * @param kinematics A correctly-configured kinematics object for your drivetrain. - * @param gyroAngle The current gyro angle. - * @param modulePositions The current distance and rotation measurements of the swerve modules. - * @param initialPoseMeters The starting pose estimate. - * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position - * in meters, and heading in radians). Increase these numbers to trust your state estimate - * less. - * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position - * in meters, y position in meters, and heading in radians). Increase these numbers to trust - * the vision pose measurement less. - */ - public SwerveDrivePoseEstimator( - SwerveDriveKinematics kinematics, - Rotation2d gyroAngle, - SwerveModulePosition[] modulePositions, - Pose2d initialPoseMeters, - Matrix stateStdDevs, - Matrix visionMeasurementStdDevs) { - super( - kinematics, - new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters), - stateStdDevs, - visionMeasurementStdDevs); + /** + * Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision + * measurements. + * + *

The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y, + * and 0.1 radians for heading. The default standard deviations of the vision measurements are 0.9 + * meters for x, 0.9 meters for y, and 0.9 radians for heading. + * + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param gyroAngle The current gyro angle. + * @param modulePositions The current distance measurements and rotations of the swerve modules. + * @param initialPoseMeters The starting pose estimate. + */ + public SwerveDrivePoseEstimator( + SwerveDriveKinematics kinematics, + Rotation2d gyroAngle, + SwerveModulePosition[] modulePositions, + Pose2d initialPoseMeters + ) { + this( + kinematics, + gyroAngle, + modulePositions, + initialPoseMeters, + VecBuilder.fill(0.1, 0.1, 0.1), + VecBuilder.fill(0.9, 0.9, 0.9)); + } - m_numModules = modulePositions.length; - } + /** + * Constructs a SwerveDrivePoseEstimator. + * + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param gyroAngle The current gyro angle. + * @param modulePositions The current distance and rotation measurements of the swerve modules. + * @param initialPoseMeters The starting pose estimate. + * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position + * in meters, and heading in radians). Increase these numbers to trust your state estimate + * less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position + * in meters, y position in meters, and heading in radians). Increase these numbers to trust + * the vision pose measurement less. + */ + public SwerveDrivePoseEstimator( + SwerveDriveKinematics kinematics, + Rotation2d gyroAngle, + SwerveModulePosition[] modulePositions, + Pose2d initialPoseMeters, + Matrix stateStdDevs, + Matrix visionMeasurementStdDevs + ) { + super( + kinematics, + new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters), + stateStdDevs, + visionMeasurementStdDevs); - /** - * Resets the robot's position on the field. - * - *

The gyroscope angle does not need to be reset in the user's robot code. The library - * automatically takes care of offsetting the gyro angle. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param modulePositions The current distance measurements and rotations of the swerve modules. - * @param poseMeters The position on the field that your robot is at. - */ - public void resetPosition( - Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters) { - resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), poseMeters); - } + m_numModules = modulePositions.length; + } - /** - * Updates the pose estimator with wheel encoder and gyro information. This should be called every - * loop. - * - * @param gyroAngle The current gyro angle. - * @param modulePositions The current distance measurements and rotations of the swerve modules. - * @return The estimated pose of the robot in meters. - */ - public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { - return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions)); - } + /** + * Resets the robot's position on the field. + * + *

The gyroscope angle does not need to be reset in the user's robot code. The library + * automatically takes care of offsetting the gyro angle. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param modulePositions The current distance measurements and rotations of the swerve modules. + * @param poseMeters The position on the field that your robot is at. + */ + public void resetPosition( + Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters + ) { + resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), poseMeters); + } - /** - * Updates the pose estimator with wheel encoder and gyro information. This should be called every - * loop. - * - * @param currentTimeSeconds Time at which this method was called, in seconds. - * @param gyroAngle The current gyroscope angle. - * @param modulePositions The current distance measurements and rotations of the swerve modules. - * @return The estimated pose of the robot in meters. - */ - public Pose2d updateWithTime( - double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { - return updateWithTime( - currentTimeSeconds, gyroAngle, new SwerveDriveWheelPositions(modulePositions)); - } + /** + * Updates the pose estimator with wheel encoder and gyro information. This should be called every + * loop. + * + * @param gyroAngle The current gyro angle. + * @param modulePositions The current distance measurements and rotations of the swerve modules. + * @return The estimated pose of the robot in meters. + */ + public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { + return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions)); + } - @Override - public Pose2d updateWithTime( - double currentTimeSeconds, Rotation2d gyroAngle, SwerveDriveWheelPositions wheelPositions) { - if (wheelPositions.positions.length != m_numModules) { - throw new IllegalArgumentException( - "Number of modules is not consistent with number of wheel locations provided in " - + "constructor"); + /** + * Updates the pose estimator with wheel encoder and gyro information. This should be called every + * loop. + * + * @param currentTimeSeconds Time at which this method was called, in seconds. + * @param gyroAngle The current gyroscope angle. + * @param modulePositions The current distance measurements and rotations of the swerve modules. + * @return The estimated pose of the robot in meters. + */ + public Pose2d updateWithTime( + double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions + ) { + return updateWithTime( + currentTimeSeconds, gyroAngle, new SwerveDriveWheelPositions(modulePositions)); } - return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions); - } + @Override + public Pose2d updateWithTime( + double currentTimeSeconds, Rotation2d gyroAngle, SwerveDriveWheelPositions wheelPositions + ) { + if (wheelPositions.positions.length != m_numModules) { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of wheel locations provided in " + + "constructor"); + } + + return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions); + } } diff --git a/src/main/java/frc/robot/subsystems/vision/PhotonVision.java b/src/main/java/frc/robot/subsystems/vision/PhotonVision.java index f8066a0..985d3a0 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhotonVision.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.Constants; import frc.robot.constants.FieldConstants; import frc.robot.subsystems.drive.Swerve; @@ -28,6 +29,8 @@ import java.util.function.Supplier; import java.util.stream.Collectors; +import static edu.wpi.first.wpilibj2.command.Commands.runOnce; + public class PhotonVision extends VirtualSubsystem { public static final String PhotonLogKey = "Vision"; @@ -254,6 +257,11 @@ private void update() { final VisionIO.VisionIOInputs inputs = visionIOInputsEntry.getValue(); final String logKey = PhotonLogKey + "/" + inputs.name; + Logger.recordOutput( + logKey + "/CameraPose", + new Pose3d(swerve.getPose()).transformBy(Constants.Vision.ROBOT_TO_REAR_NOTE) + ); + final EstimatedRobotPose estimatedRobotPose = runner.getEstimatedRobotPose(visionIO); if (estimatedRobotPose != null) { final EstimatedRobotPose lastEstimatedPose = lastEstimatedRobotPose.get(visionIO); @@ -285,6 +293,11 @@ private void update() { final VisionIO.VisionIOInputs inputs = visionIOInputsEntry.getValue(); final String logKey = PhotonLogKey + "/" + inputs.name; + Logger.recordOutput( + logKey + "/CameraPose", + new Pose3d(swerve.getPose()).transformBy(Constants.Vision.ROBOT_TO_REAR_NOTE) + ); + final NoteTrackingResult noteTrackingResult = runner.getNoteTrackingResult(visionIO); if (noteTrackingResult != null) { Logger.recordOutput(logKey + "/HasTargets", noteTrackingResult.hasTargets); @@ -296,10 +309,6 @@ private void update() { logKey + "/BestNotePose", new Pose3d(optionalBestNotePose.orElseGet(Pose2d::new)) ); - Logger.recordOutput( - "NoteCameraPose", - new Pose3d(swerve.getPose()).transformBy(Constants.Vision.ROBOT_TO_REAR_NOTE) - ); final Pose2d[] notePose2ds = noteTrackingResult .getNotePoses(swerve::getPose); @@ -380,6 +389,10 @@ public void resetPosition(final Pose2d robotPose) { resetPosition(robotPose, swerve.getYaw()); } + public Command resetPoseCommand(final Pose2d robotPose) { + return runOnce(() -> resetPosition(robotPose)); + } + public List getNotePoses() { final List notePoses = new ArrayList<>(); for (final VisionIO visionIO : noteTrackingVisionIOInputsMap.keySet()) { diff --git a/src/test/java/frc/robot/subsystems/drive/SwerveTest.java b/src/test/java/frc/robot/subsystems/drive/SwerveTest.java index e1e17f6..461e299 100644 --- a/src/test/java/frc/robot/subsystems/drive/SwerveTest.java +++ b/src/test/java/frc/robot/subsystems/drive/SwerveTest.java @@ -2,7 +2,6 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -11,6 +10,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import frc.robot.constants.Constants; import frc.robot.subsystems.drive.constants.SwerveConstants; +import frc.robot.subsystems.drive.estimator.SwerveDrivePoseEstimator; import frc.robot.subsystems.gyro.Gyro; import org.junit.jupiter.api.BeforeAll; import org.junit.jupiter.api.BeforeEach;