diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2005300..c06a1ad 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -65,10 +65,10 @@ public static final class KrakenX60Constants { } public static final class AutoConstants { - public static final int kAllianceColorSelectorPort = 10; + public static final int kAllianceColorSelectorPort = 3; // max length is 8 - public static final int[] kAutonomousModeSelectorPorts = {11, 12, 13, 18, 19}; + public static final int[] kAutonomousModeSelectorPorts = {0, 1, 2}; } public static final class OIConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1e2831c..8b596c8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -3,7 +3,6 @@ import static edu.wpi.first.units.Units.Meters; import static frc.robot.subsystems.vision.VisionConstants.*; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -97,7 +96,7 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - drive::getPose, + drive::getVisionPose, new VisionIOPhotonVision(cameraFrontRightName, robotToFrontRightCamera), new VisionIOPhotonVision(cameraFrontLeftName, robotToFrontLeftCamera), new VisionIOPhotonVision(cameraBackRightName, robotToBackRightCamera), @@ -119,15 +118,15 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - drive::getPose, + drive::getOdometryPose, new VisionIOPhotonVisionSim( - cameraFrontRightName, robotToFrontRightCamera, drive::getPose), + cameraFrontRightName, robotToFrontRightCamera, drive::getOdometryPose), new VisionIOPhotonVisionSim( - cameraFrontLeftName, robotToFrontLeftCamera, drive::getPose), + cameraFrontLeftName, robotToFrontLeftCamera, drive::getOdometryPose), new VisionIOPhotonVisionSim( - cameraBackRightName, robotToBackRightCamera, drive::getPose), + cameraBackRightName, robotToBackRightCamera, drive::getOdometryPose), new VisionIOPhotonVisionSim( - cameraBackLeftName, robotToBackLeftCamera, drive::getPose)); + cameraBackLeftName, robotToBackLeftCamera, drive::getOdometryPose)); break; case REPLAY: // Replaying a log @@ -149,7 +148,7 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - drive::getPose, + drive::getOdometryPose, new VisionIO() {}, new VisionIO() {}, new VisionIO() {}, @@ -286,8 +285,10 @@ public void bindZorroDriver(int port) { .onTrue( Commands.runOnce( () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), + drive.setOdometryRotation( + allianceSelector.fieldRotated() + ? Rotation2d.k180deg + : Rotation2d.kZero), drive) .ignoringDisable(true)); @@ -318,8 +319,10 @@ public void bindXboxDriver(int port) { .onTrue( Commands.runOnce( () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), + drive.setOdometryRotation( + allianceSelector.fieldRotated() + ? Rotation2d.k180deg + : Rotation2d.kZero), drive) .ignoringDisable(true)); diff --git a/src/main/java/frc/robot/auto/AutoMode.java b/src/main/java/frc/robot/auto/AutoMode.java index 2a6e868..4441334 100644 --- a/src/main/java/frc/robot/auto/AutoMode.java +++ b/src/main/java/frc/robot/auto/AutoMode.java @@ -14,8 +14,8 @@ public abstract class AutoMode { public AutoMode(Drive drivetrain) { autoFactory = new AutoFactory( - drivetrain::getPose, - drivetrain::setPose, + drivetrain::getOdometryPose, + drivetrain::setOdometryPose, drivetrain::followTrajectory, false, drivetrain); diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 35f49fe..b059635 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -99,8 +99,8 @@ public static Command joystickDrive( ChassisSpeeds.fromFieldRelativeSpeeds( speeds, fieldRotatedSupplier.getAsBoolean() - ? drive.getRotation().plus(Rotation2d.kPi) - : drive.getRotation()); + ? drive.getOdometryRotation().plus(Rotation2d.kPi) + : drive.getOdometryRotation()); } drive.runVelocity(speeds); @@ -140,8 +140,8 @@ public static Command fieldRelativeJoystickDrive( ChassisSpeeds.fromFieldRelativeSpeeds( speeds, fieldRotatedSupplier.getAsBoolean() - ? drive.getRotation().plus(Rotation2d.kPi) - : drive.getRotation())); + ? drive.getOdometryRotation().plus(Rotation2d.kPi) + : drive.getOdometryRotation())); }, drive) .withName("Field Relative Joystick Drive"); @@ -209,7 +209,8 @@ public static Command joystickDriveAtFixedOrientation( // Calculate angular speed double omega = angleController.calculate( - drive.getRotation().getRadians(), rotationSupplier.get().getRadians()); + drive.getOdometryRotation().getRadians(), + rotationSupplier.get().getRadians()); // Convert to field relative speeds & send command ChassisSpeeds speeds = @@ -221,14 +222,14 @@ public static Command joystickDriveAtFixedOrientation( ChassisSpeeds.fromFieldRelativeSpeeds( speeds, fieldRotatedSupplier.getAsBoolean() - ? drive.getRotation().plus(Rotation2d.kPi) - : drive.getRotation())); + ? drive.getOdometryRotation().plus(Rotation2d.kPi) + : drive.getOdometryRotation())); }, drive) .withName("Fixed Orientation Joystick Drive") // Reset PID controller when command starts - .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); + .beforeStarting(() -> angleController.reset(drive.getOdometryRotation().getRadians())); } /** @@ -261,8 +262,8 @@ public static Command pointAtTarget( ChassisSpeeds.fromFieldRelativeSpeeds( speeds, fieldRotatedSupplier.getAsBoolean() - ? drive.getRotation().plus(Rotation2d.kPi) - : drive.getRotation())); + ? drive.getOdometryRotation().plus(Rotation2d.kPi) + : drive.getOdometryRotation())); }, drive) .withName("Point At Target") @@ -365,14 +366,14 @@ public static Command wheelRadiusCharacterization(Drive drive) { Commands.runOnce( () -> { state.positions = drive.getWheelRadiusCharacterizationPositions(); - state.lastAngle = drive.getRotation(); + state.lastAngle = drive.getOdometryRotation(); state.gyroDelta = 0.0; }), // Update gyro delta Commands.run( () -> { - var rotation = drive.getRotation(); + var rotation = drive.getOdometryRotation(); state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); state.lastAngle = rotation; }) diff --git a/src/main/java/frc/robot/commands/PathCommands.java b/src/main/java/frc/robot/commands/PathCommands.java index 87ff9e0..119bc1d 100644 --- a/src/main/java/frc/robot/commands/PathCommands.java +++ b/src/main/java/frc/robot/commands/PathCommands.java @@ -38,7 +38,7 @@ public static Command goToTargetPose(Drive drive, Pose2d targetPose) { @Override public Command get() { List waypoints = - PathPlannerPath.waypointsFromPoses(drive.getPose(), targetPose); + PathPlannerPath.waypointsFromPoses(drive.getVisionPose(), targetPose); return AutoBuilder.followPath(createPath(waypoints, targetPose.getRotation())); } @@ -62,7 +62,7 @@ public static Command dockToTargetPoint( new Supplier() { @Override public Command get() { - var initialPose = drive.getPose(); + var initialPose = drive.getVisionPose(); var heading = targetPoint.minus(initialPose.getTranslation()).getAngle(); var finalPose = new Pose2d(targetPoint, heading); List waypoints = @@ -95,7 +95,7 @@ public static Command dockToTargetPose(Drive drive, Pose2d targetPose, Distance public Command get() { List waypoints = PathPlannerPath.waypointsFromPoses( - drive.getPose(), + drive.getVisionPose(), targetPose.plus( new Transform2d(leadDistance.unaryMinus(), Meters.of(0), Rotation2d.kZero)), targetPose); @@ -118,7 +118,7 @@ public static Command advanceForward(Drive drive, Distance leadDistance) { new Supplier() { @Override public Command get() { - var initialPose = drive.getPose(); + var initialPose = drive.getVisionPose(); var waypoints = PathPlannerPath.waypointsFromPoses( initialPose, diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index f525347..4668fdd 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -75,7 +75,9 @@ public class Drive extends SubsystemBase { new SwerveModulePosition(), new SwerveModulePosition() }; - private SwerveDrivePoseEstimator poseEstimator = + private SwerveDrivePoseEstimator odometryPose = + new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); + private SwerveDrivePoseEstimator visionPose = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); private Boolean firstVisionEstimate = true; @@ -104,8 +106,8 @@ public Drive( // Configure AutoBuilder for PathPlanner AutoBuilder.configure( - this::getPose, - this::setPose, + this::getVisionPose, + this::setOdometryPose, this::getChassisSpeeds, this::runVelocity, new PPHolonomicDriveController( @@ -190,7 +192,8 @@ public void periodic() { } // Apply update - poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); + odometryPose.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); + visionPose.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); } // Update gyro alert @@ -223,7 +226,7 @@ public void runVelocity(ChassisSpeeds speeds) { public void followTrajectory(SwerveSample sample) { // Get the current pose of the robot - Pose2d pose = getPose(); + Pose2d pose = getVisionPose(); // Generate the next speeds for the robot ChassisSpeeds speeds = @@ -234,7 +237,7 @@ public void followTrajectory(SwerveSample sample) { + headingController.calculate(pose.getRotation().getRadians(), sample.heading)); // Apply the generated speeds - runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getRotation())); + runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getOdometryRotation())); } /** Runs the drive in a straight line with the specified drive output. */ @@ -318,19 +321,30 @@ public double getFFCharacterizationVelocity() { } /** Returns the current odometry pose. */ - @AutoLogOutput(key = "Odometry/Robot") - public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); + @AutoLogOutput(key = "Localizer/Odometry") + public Pose2d getOdometryPose() { + return odometryPose.getEstimatedPosition(); + } + + /** Returns the current vision pose. */ + @AutoLogOutput(key = "Localizer/Vision") + public Pose2d getVisionPose() { + return visionPose.getEstimatedPosition(); } /** Returns the current odometry rotation. */ - public Rotation2d getRotation() { - return getPose().getRotation(); + public Rotation2d getOdometryRotation() { + return getOdometryPose().getRotation(); + } + + /** Sets the current odometry orientation. */ + public void setOdometryRotation(Rotation2d rotation) { + setOdometryPose(new Pose2d(getOdometryPose().getTranslation(), rotation)); } /** Resets the current odometry pose. */ - public void setPose(Pose2d pose) { - poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + public void setOdometryPose(Pose2d pose) { + odometryPose.resetPosition(rawGyroRotation, getModulePositions(), pose); } /** Adds a new timestamped vision measurement. */ @@ -340,11 +354,11 @@ public void addVisionMeasurement( Matrix visionMeasurementStdDevs) { // Teleport the odometry to the first vision estimate if (firstVisionEstimate && RobotState.isDisabled()) { - setPose(visionRobotPoseMeters); + setOdometryPose(visionRobotPoseMeters); firstVisionEstimate = false; } - poseEstimator.addVisionMeasurement( + visionPose.addVisionMeasurement( visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); }