Add Phoenix Pro features

src/main/java/org/team1540/robot2024/Constants.java

@@ -1,6 +1,8 @@
 package org.team1540.robot2024;
 
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.util.Units;
 
 /**
@@ -44,6 +46,9 @@ public static class SwerveConfig {
         public static final int FRONT_RIGHT = IS_COMPETITION_ROBOT ? 4 : 0;
         public static final int BACK_LEFT   = IS_COMPETITION_ROBOT ? 7 : 0;
         public static final int BACK_RIGHT  = IS_COMPETITION_ROBOT ? 1 : 0;
+
+        // TODO: set this id
+        public static final int PIGEON_ID = 0;
     }
 
     public static class Drivetrain {
@@ -59,6 +64,30 @@ public static class Drivetrain {
         public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
     }
 
+
+    public static class Vision {
+        public static final String FRONT_CAMERA_NAME = "limelight-front";
+        public static final String REAR_CAMERA_NAME = "limelight-rear";
+
+        // TODO: measure these offsets
+        public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0, 0, 0.5, new Rotation3d());
+        public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0, 0, 0.5, new Rotation3d(0, 0, Math.PI));
+
+        // TODO: find these values
+        public static final double MAX_VISION_DELAY_SECS = 0.08;
+        public static final double MAX_ACCEPTED_ROT_SPEED_RAD_PER_SEC = 1.0;
+        public static final double MAX_ACCEPTED_LINEAR_SPEED_MPS = 4.0;
+        public static final double MIN_ACCEPTED_NUM_TAGS = 1;
+        public static final double MAX_ACCEPTED_AVG_TAG_DIST_METERS = 8.0;
+        public static final double MAX_ACCEPTED_ELEVATOR_SPEED_MPS = 0.05;
+
+        public static final int SIM_RES_WIDTH = 1280;
+        public static final int SIM_RES_HEIGHT = 960;
+        public static final Rotation2d SIM_DIAGONAL_FOV = Rotation2d.fromDegrees(100);
+        public static final double SIM_FPS = 14.5;
+        public static final double SIM_AVG_LATENCY_MS = 67.0;
+    }
+
     public static class Shooter {
         public static class Flywheels {
             // TODO: determine ids

src/main/java/org/team1540/robot2024/Robot.java

@@ -90,6 +90,7 @@ public void robotPeriodic() {
         // This must be called from the robot's periodic block in order for anything in
         // the Command-based framework to work.
         CommandScheduler.getInstance().run();
+        if (Constants.currentMode == Constants.Mode.REAL) robotContainer.odometrySignalRefresher.periodic();
 
         // Update mechanism visualiser in sim
         if (Robot.isSimulation()) MechanismVisualiser.periodic();

src/main/java/org/team1540/robot2024/RobotContainer.java

@@ -18,8 +18,14 @@
 import org.team1540.robot2024.subsystems.tramp.TrampIOSim;
 import org.team1540.robot2024.subsystems.tramp.TrampIOSparkMax;
 import org.team1540.robot2024.subsystems.shooter.*;
+import org.team1540.robot2024.subsystems.vision.AprilTagVision;
+import org.team1540.robot2024.subsystems.vision.AprilTagVisionIO;
+import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOLimelight;
+import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOSim;
+import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
 import org.team1540.robot2024.util.swerve.SwerveFactory;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+import org.team1540.robot2024.util.vision.VisionPoseAcceptor;
 
 import static org.team1540.robot2024.Constants.SwerveConfig;
 
@@ -34,6 +40,7 @@ public class RobotContainer {
     public final Drivetrain drivetrain;
     public final Tramp tramp;
     public final Shooter shooter;
+    public final AprilTagVision aprilTagVision;
 
     // Controller
     public final CommandXboxController driver = new CommandXboxController(0);
@@ -42,6 +49,8 @@ public class RobotContainer {
     // Dashboard inputs
     public final LoggedDashboardChooser<Command> autoChooser;
 
+    public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS);
+
     // TODO: testing dashboard inputs, remove for comp
     public final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000);
     public final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000);
@@ -55,13 +64,19 @@ public RobotContainer() {
                 // Real robot, instantiate hardware IO implementations
                 drivetrain =
                         new Drivetrain(
-                                new GyroIONavx(),
-                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT)),
-                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT)),
-                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT)),
-                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT)));
+                                new GyroIOPigeon2(odometrySignalRefresher),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT), odometrySignalRefresher),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT), odometrySignalRefresher),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT), odometrySignalRefresher),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), odometrySignalRefresher));
                 tramp = new Tramp(new TrampIOSparkMax());
                 shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
+                aprilTagVision = new AprilTagVision(
+                        new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE),
+                        new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE),
+                        drivetrain::addVisionMeasurement,
+                        () -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
+                        new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
                 break;
             case SIM:
                 // Sim robot, instantiate physics sim IO implementations
@@ -74,8 +89,14 @@ public RobotContainer() {
                                 new ModuleIOSim());
                 tramp = new Tramp(new TrampIOSim());
                 shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim());
+                aprilTagVision =
+                        new AprilTagVision(
+                                new AprilTagVisionIOSim(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE, drivetrain::getPose),
+                                new AprilTagVisionIOSim(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE, drivetrain::getPose),
+                                ignored -> {},
+                                () -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
+                                new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
                 break;
-
             default:
                 // Replayed robot, disable IO implementations
                 drivetrain =
@@ -86,6 +107,13 @@ public RobotContainer() {
                                 new ModuleIO() {},
                                 new ModuleIO() {});
                 shooter = new Shooter(new ShooterPivotIO() {}, new FlywheelsIO() {});
+                aprilTagVision =
+                        new AprilTagVision(
+                                new AprilTagVisionIO() {},
+                                new AprilTagVisionIO() {},
+                                (ignored) -> {},
+                                () -> 0.0,
+                                new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0));
                 tramp = new Tramp(new TrampIO() {});
                 break;
         }

src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java

@@ -5,10 +5,11 @@
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PathPlannerLogging;
 import com.pathplanner.lib.util.ReplanningConfig;
+import edu.wpi.first.math.VecBuilder;
+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.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -18,6 +19,7 @@
 import org.team1540.robot2024.util.LocalADStarAK;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
+import org.team1540.robot2024.util.vision.TimestampedVisionPose;
 
 import static org.team1540.robot2024.Constants.Drivetrain.*;
 
@@ -27,10 +29,11 @@ public class Drivetrain extends SubsystemBase {
     private final Module[] modules = new Module[4]; // FL, FR, BL, BR
 
     private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
-    private Pose2d pose = new Pose2d();
-    private Rotation2d lastGyroRotation = new Rotation2d();
+    private Rotation2d rawGyroRotation = new Rotation2d();
     private boolean forceModuleAngleChange = false;
 
+    private final SwerveDrivePoseEstimator poseEstimator;
+
     public Drivetrain(
             GyroIO gyroIO,
             ModuleIO flModuleIO,
@@ -43,6 +46,15 @@ public Drivetrain(
         modules[2] = new Module(blModuleIO, 2);
         modules[3] = new Module(brModuleIO, 3);
 
+        poseEstimator = new SwerveDrivePoseEstimator(
+                kinematics,
+                rawGyroRotation,
+                getModulePositions(),
+                new Pose2d(),
+                // TODO: tune std devs (scale vision by distance?)
+                VecBuilder.fill(0.1, 0.1, 0.1),
+                VecBuilder.fill(0.5, 0.5, 5.0)); // Trust the gyro more than the AprilTags
+
         // Configure AutoBuilder for PathPlanner
         AutoBuilder.configureHolonomic(
                 this::getPose,
@@ -60,44 +72,35 @@ public Drivetrain(
                 (targetPose) -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose));
     }
 
+    @Override
     public void periodic() {
         gyroIO.updateInputs(gyroInputs);
         Logger.processInputs("Drivetrain/Gyro", gyroInputs);
         for (Module module : modules) {
             module.periodic();
         }
 
-        // Stop moving when disabled
-        if (DriverStation.isDisabled()) {
-            for (Module module : modules) {
-                module.stop();
-            }
-        }
-        // Log empty setpoint states when disabled
         if (DriverStation.isDisabled()) {
+            // Stop moving when disabled
+            for (Module module : modules) module.stop();
+
+            // Log empty setpoint states when disabled
             Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[]{});
             Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[]{});
         }
 
-        // Update odometry
+        // Calculate module deltas
         SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4];
         for (int i = 0; i < 4; i++) {
             wheelDeltas[i] = modules[i].getPositionDelta();
         }
-        // The twist represents the motion of the robot since the last
-        // loop cycle in x, y, and theta based only on the modules,
-        // without the gyro. The gyro is always disconnected in simulation.
-        Twist2d twist = kinematics.toTwist2d(wheelDeltas);
-        if (gyroInputs.connected) {
-            // If the gyro is connected, replace the theta component of the twist
-            // with the change in angle since the last loop cycle.
-            twist =
-                    new Twist2d(
-                            twist.dx, twist.dy, gyroInputs.yawPosition.minus(lastGyroRotation).getRadians());
-            lastGyroRotation = gyroInputs.yawPosition;
-        }
-        // Apply the twist (change since last loop cycle) to the current pose
-        pose = pose.exp(twist);
+        // Use gyro rotation if gyro is connected, otherwise use module deltas to calculate rotation delta
+        rawGyroRotation =
+                gyroInputs.connected ?
+                        gyroInputs.yawPosition
+                        : rawGyroRotation.plus(Rotation2d.fromRadians(kinematics.toTwist2d(wheelDeltas).dtheta));
+        // Update odometry
+        poseEstimator.update(rawGyroRotation, getModulePositions());
     }
 
     /**
@@ -164,6 +167,10 @@ public double getCharacterizationVelocity() {
         return driveVelocityAverage / 4.0;
     }
 
+    public ChassisSpeeds getChassisSpeeds() {
+        return kinematics.toChassisSpeeds(getModuleStates());
+    }
+
     /**
      * Returns the module states (turn angles and drive velocities) for all the modules.
      */
@@ -181,21 +188,34 @@ private SwerveModuleState[] getModuleStates() {
      */
     @AutoLogOutput(key = "Odometry/Robot")
     public Pose2d getPose() {
-        return pose;
+        return poseEstimator.getEstimatedPosition();
     }
 
     /**
      * Returns the current odometry rotation.
      */
     public Rotation2d getRotation() {
-        return pose.getRotation();
+        return getPose().getRotation();
     }
 
     /**
      * Resets the current odometry pose.
      */
     public void setPose(Pose2d pose) {
-        this.pose = pose;
+        poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose);
+    }
+
+    public void addVisionMeasurement(TimestampedVisionPose visionPose) {
+        poseEstimator.addVisionMeasurement(visionPose.poseMeters(), visionPose.timestampSecs());
+    }
+
+    public SwerveModulePosition[] getModulePositions() {
+        return new SwerveModulePosition[]{
+                modules[0].getPosition(),
+                modules[1].getPosition(),
+                modules[2].getPosition(),
+                modules[3].getPosition(),
+        };
     }
 
     /**

src/main/java/org/team1540/robot2024/subsystems/drive/GyroIOPigeon2.java

@@ -1,32 +1,40 @@
 package org.team1540.robot2024.subsystems.drive;
 
 import com.ctre.phoenix6.BaseStatusSignal;
-import com.ctre.phoenix6.StatusCode;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import com.ctre.phoenix6.hardware.Pigeon2;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
+import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
+
+import static org.team1540.robot2024.Constants.SwerveConfig.*;
 
 /**
  * IO implementation for Pigeon2
  */
 public class GyroIOPigeon2 implements GyroIO {
-    private final Pigeon2 pigeon = new Pigeon2(20);
+    private final Pigeon2 pigeon = new Pigeon2(PIGEON_ID);
     private final StatusSignal<Double> yaw = pigeon.getYaw();
     private final StatusSignal<Double> yawVelocity = pigeon.getAngularVelocityZDevice();
 
-    public GyroIOPigeon2() {
+    private final PhoenixTimeSyncSignalRefresher odometrySignalRefresher;
+
+    public GyroIOPigeon2(PhoenixTimeSyncSignalRefresher odometrySignalRefresher) {
         pigeon.getConfigurator().apply(new Pigeon2Configuration());
         pigeon.getConfigurator().setYaw(0.0);
         yaw.setUpdateFrequency(100.0);
         yawVelocity.setUpdateFrequency(100.0);
         pigeon.optimizeBusUtilization();
+
+        this.odometrySignalRefresher = odometrySignalRefresher;
+        odometrySignalRefresher.registerSignals(yaw, yawVelocity);
     }
 
     @Override
     public void updateInputs(GyroIOInputs inputs) {
-        inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK);
+        odometrySignalRefresher.refreshSignals();
+        inputs.connected = BaseStatusSignal.isAllGood(yaw, yawVelocity);
         inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
         inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
     }

src/main/java/org/team1540/robot2024/subsystems/drive/Module.java

@@ -20,8 +20,6 @@ public class Module {
     private final PIDController turnFeedback;
     private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop
     private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop
-    private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute
-    private boolean forceTurn = false;
     private double lastPositionMeters = 0.0; // Used for delta calculation
 
     public Module(ModuleIO io, int index) {
@@ -55,17 +53,10 @@ public Module(ModuleIO io, int index) {
 
     public void periodic() {
         io.updateInputs(inputs);
-        Logger.processInputs("Drive/Module" + index, inputs);
-
-        // On first cycle, reset relative turn encoder
-        // Wait until absolute angle is nonzero in case it wasn't initialized yet
-        if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) {
-            turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition);
-        }
+        Logger.processInputs("Drivetrain/Module" + index, inputs);
 
         // Run closed loop turn control
         if (angleSetpoint != null) {
-
             io.setTurnVoltage(
                     turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians()));
 
@@ -143,11 +134,7 @@ public void setBrakeMode(boolean enabled) {
      * Returns the current turn angle of the module.
      */
     public Rotation2d getAngle() {
-        if (turnRelativeOffset == null) {
-            return new Rotation2d();
-        } else {
-            return inputs.turnPosition.plus(turnRelativeOffset);
-        }
+        return inputs.turnPosition;
     }
 
     /**