update 2024 code to new SparkMax, Pigeon, CANcoder APIs.

src/main/java/com/team766/hal/GyroReader.java

@@ -2,7 +2,7 @@
 
 public interface GyroReader {
     enum Type {
-        PIGEON,
+        Pigeon2,
     }
 
     /**

src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java

@@ -5,7 +5,7 @@
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.REVLibError;
 import com.revrobotics.RelativeEncoder;
-import com.revrobotics.SparkMaxAnalogSensor;
+import com.revrobotics.SparkAnalogSensor;
 import com.team766.hal.MotorController;
 import com.team766.hal.MotorControllerCommandFailedException;
 import com.team766.logging.LoggerExceptionUtils;
@@ -149,7 +149,7 @@ public void setSelectedFeedbackSensor(final FeedbackDevice feedbackDevice) {
         switch (feedbackDevice) {
             case Analog:
                 {
-                    SparkMaxAnalogSensor analog = getAnalog(SparkMaxAnalogSensor.Mode.kAbsolute);
+                    SparkAnalogSensor analog = getAnalog(SparkAnalogSensor.Mode.kAbsolute);
                     revErrorToException(ExceptionTarget.LOG, analog.setInverted(sensorInverted));
                     sensorPositionSupplier = analog::getPosition;
                     sensorVelocitySupplier = analog::getVelocity;

src/main/java/com/team766/hal/wpilib/PigeonGyro.java

@@ -1,9 +1,16 @@
 package com.team766.hal.wpilib;
 
-import com.ctre.phoenix.ErrorCode;
-import com.ctre.phoenix.sensors.Pigeon2;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.hardware.Pigeon2;
 import com.team766.hal.GyroReader;
-
+import com.team766.logging.Category;
+import com.team766.logging.Logger;
+import com.team766.logging.Severity;
+
+// TODO: add support for configuring the Pigeon2's mountpose
+// https://api.ctr-electronics.com/phoenix6/release/java/com/ctre/phoenix6/hardware/core/CorePigeon2.html
+// TODO: add support for getting the robot's heading as a Rotation2d, etc.
+// https://api.ctr-electronics.com/phoenix6/release/java/com/ctre/phoenix6/hardware/Pigeon2.html
 public class PigeonGyro implements GyroReader {
     private final Pigeon2 pigeon;
 
@@ -12,9 +19,7 @@ public PigeonGyro(int canId, String canBus) {
     }
 
     @Override
-    public void calibrate() {
-        pigeon.zeroGyroBiasNow();
-    }
+    public void calibrate() {}
 
     @Override
     public void reset() {
@@ -23,23 +28,33 @@ public void reset() {
 
     @Override
     public double getAngle() {
-        return pigeon.getYaw();
+        return pigeon.getAngle();
     }
 
     @Override
     public double getPitch() {
-        return pigeon.getPitch();
+        StatusSignal<Double> value = pigeon.getPitch();
+        if (value.getStatus().isOK()) {
+            return value.getValueAsDouble();
+        }
+        Logger.get(Category.GYRO)
+                .logData(Severity.ERROR, "Unable to get pitch: %s", value.toString());
+        return 0;
     }
 
     @Override
     public double getRoll() {
-        return pigeon.getRoll();
+        StatusSignal<Double> value = pigeon.getRoll();
+        if (value.getStatus().isOK()) {
+            return value.getValueAsDouble();
+        }
+        Logger.get(Category.GYRO)
+                .logData(Severity.ERROR, "Unable to get roll: %s", value.toString());
+        return 0;
     }
 
     @Override
     public double getRate() {
-        double[] xyz = new double[3];
-        ErrorCode eCode = pigeon.getRawGyro(xyz);
-        return eCode == ErrorCode.OK ? xyz[2] : 0;
+        return pigeon.getRate();
     }
 }

src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java

@@ -192,7 +192,7 @@ public GyroReader getGyro(final int index, String configPrefix) {
                         .getEnum(GyroReader.Type.class, configPrefix + ".type");
 
         if (type.hasValue()) {
-            if (type.get() == GyroReader.Type.PIGEON) {
+            if (type.get() == GyroReader.Type.Pigeon2) {
                 ValueProvider<String> canBus =
                         ConfigFileReader.getInstance().getString(configPrefix + ".CANBus");
                 return new PigeonGyro(index, canBus.get());

src/main/java/com/team766/odometry/Odometry.java

@@ -1,11 +1,14 @@
 package com.team766.odometry;
 
-import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.hardware.CANcoder;
 import com.team766.framework.LoggingBase;
 import com.team766.hal.GyroReader;
 import com.team766.hal.MotorController;
 import com.team766.library.RateLimiter;
 import com.team766.logging.Category;
+import com.team766.logging.Logger;
+import com.team766.logging.Severity;
 import com.team766.robot.gatorade.*;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -22,7 +25,7 @@ public class Odometry extends LoggingBase {
     private GyroReader gyro;
     private MotorController[] motorList;
     // The order of CANCoders should be the same as in motorList
-    private CANCoder[] CANCoderList;
+    private CANcoder[] CANCoderList;
     private int motorCount;
 
     private PointDir[] prevPositions;
@@ -55,7 +58,7 @@ public class Odometry extends LoggingBase {
     public Odometry(
             GyroReader gyro,
             MotorController[] motors,
-            CANCoder[] CANCoders,
+            CANcoder[] CANCoders,
             Point[] wheelLocations,
             double wheelCircumference,
             double gearRatio,
@@ -140,6 +143,18 @@ private void updateCurrentPositions() {
         */
 
         for (int i = 0; i < motorCount; i++) {
+
+            StatusSignal<Double> positionStatus = CANCoderList[i].getAbsolutePosition();
+            if (!positionStatus.getStatus().isOK()) {
+                Logger.get(Category.ODOMETRY)
+                        .logData(
+                                Severity.WARNING,
+                                "Unable to read CANCoder: %s",
+                                positionStatus.getStatus().toString());
+                continue;
+            }
+            double absolutePosition = positionStatus.getValueAsDouble();
+
             // prevPositions[i] = new PointDir(currentPosition.getX() + 0.5 *
             // DISTANCE_BETWEEN_WHEELS / Math.sin(Math.PI / motorCount) *
             // Math.cos(currentPosition.getHeading() + ((Math.PI + 2 * Math.PI * i) / motorCount)),
@@ -149,14 +164,14 @@ private void updateCurrentPositions() {
             // This following line only works if the average of wheel positions is (0,0)
             prevPositions[i].set(
                     currentPosition.add(wheelPositions[i]), currPositions[i].getHeading());
-            currPositions[i].setHeading(-CANCoderList[i].getAbsolutePosition() + gyroPosition);
+            currPositions[i].setHeading(-absolutePosition + gyroPosition);
             angleChange = currPositions[i].getHeading() - prevPositions[i].getHeading();
 
             double yaw = -Math.toRadians(gyro.getAngle());
             double roll = Math.toRadians(gyro.getRoll());
             double pitch = Math.toRadians(gyro.getPitch());
 
-            double w = Math.toRadians(CANCoderList[i].getAbsolutePosition());
+            double w = Math.toRadians(absolutePosition);
             Vector2D u =
                     new Vector2D(Math.cos(yaw) * Math.cos(pitch), Math.sin(yaw) * Math.cos(pitch));
             Vector2D v =

src/main/java/com/team766/robot/common/mechanisms/Drive.java

@@ -2,7 +2,7 @@
 
 import static com.team766.robot.gatorade.constants.ConfigConstants.*;
 
-import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix6.hardware.CANcoder;
 import com.team766.framework.Mechanism;
 import com.team766.hal.GyroReader;
 import com.team766.hal.MotorController;
@@ -48,10 +48,10 @@ public Drive() {
         MotorController steerBL = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_LEFT);
 
         // create the encoders
-        CANCoder encoderFR = new CANCoder(2, SwerveDriveConstants.SWERVE_CANBUS);
-        CANCoder encoderFL = new CANCoder(4, SwerveDriveConstants.SWERVE_CANBUS);
-        CANCoder encoderBR = new CANCoder(3, SwerveDriveConstants.SWERVE_CANBUS);
-        CANCoder encoderBL = new CANCoder(1, SwerveDriveConstants.SWERVE_CANBUS);
+        CANcoder encoderFR = new CANcoder(2, SwerveDriveConstants.SWERVE_CANBUS);
+        CANcoder encoderFL = new CANcoder(4, SwerveDriveConstants.SWERVE_CANBUS);
+        CANcoder encoderBR = new CANcoder(3, SwerveDriveConstants.SWERVE_CANBUS);
+        CANcoder encoderBL = new CANcoder(1, SwerveDriveConstants.SWERVE_CANBUS);
 
         // initialize the swerve modules
         swerveFR = new SwerveModule("FR", driveFR, steerFR, encoderFR);
@@ -64,7 +64,7 @@ public Drive() {
 
         currentPosition = new PointDir(0, 0, 0);
         MotorController[] motorList = new MotorController[] {driveFR, driveFL, driveBL, driveBR};
-        CANCoder[] encoderList = new CANCoder[] {encoderFR, encoderFL, encoderBL, encoderBR};
+        CANcoder[] encoderList = new CANcoder[] {encoderFR, encoderFL, encoderBL, encoderBR};
         Point[] wheelPositions =
                 new Point[] {
                     new Point(

src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java

@@ -1,8 +1,12 @@
 package com.team766.robot.common.mechanisms;
 
-import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.hardware.CANcoder;
 import com.team766.hal.MotorController;
 import com.team766.hal.MotorController.ControlMode;
+import com.team766.logging.Category;
+import com.team766.logging.Logger;
+import com.team766.logging.Severity;
 import com.team766.robot.gatorade.constants.SwerveDriveConstants;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;
@@ -15,7 +19,7 @@ public class SwerveModule {
     private final String modulePlacement;
     private final MotorController drive;
     private final MotorController steer;
-    private final CANCoder encoder;
+    private final CANcoder encoder;
     private final double offset;
 
     /*
@@ -38,7 +42,7 @@ public SwerveModule(
             String modulePlacement,
             MotorController drive,
             MotorController steer,
-            CANCoder encoder) {
+            CANcoder encoder) {
         this.modulePlacement = modulePlacement;
         this.drive = drive;
         this.steer = steer;
@@ -51,8 +55,18 @@ public SwerveModule(
     }
 
     private double computeEncoderOffset() {
+        StatusSignal<Double> value = encoder.getAbsolutePosition();
+        if (!value.getStatus().isOK()) {
+            Logger.get(Category.DRIVE)
+                    .logData(
+                            Severity.ERROR,
+                            "%s unable to read encoder: %s",
+                            modulePlacement,
+                            value.getStatus().toString());
+            return 0; // ??
+        }
         return (steer.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360
-                - encoder.getAbsolutePosition();
+                - value.getValueAsDouble();
     }
 
     /**