reverting

rio/Pipfile

@@ -7,7 +7,8 @@ name = "pypi"
 wpilib = {extras = ["all"]}
 robotpy = "2024.2.1.1"
 robotpy-rev = "2024.2.0"
-robotpy-ctre = "2024.1.2"
+robotpy-ctre = "2024.1.1"
+robotpy-navx = "2024.1.0"
 robotpy-commands-v2 = "2024.2.1"
 
 [dev-packages]

rio/constants.py

@@ -190,12 +190,3 @@ class IntakeConstants:
     kLiftI = 0.00001
     kLiftD = 1.6
     kLiftFF = 0
-
-
-class GyroConstants:
-    id = 0
-
-    # pose
-    yawPose = 0
-    pitchPose = 0
-    rollPose = 0

rio/pyproject.toml

@@ -15,7 +15,7 @@ robotpy_extras = [
     # "apriltag",
     "commands2",
     # "cscore",
-    # "navx",
+    "navx",
     # "pathplannerlib",
     "phoenix5",
     # "phoenix6",

rio/subsystems/drivesubsystem.py

@@ -13,11 +13,11 @@
     SwerveDrive4Odometry,
 )
 
-from phoenix5.sensors import Pigeon2
+from navx import AHRS
 
 from ntcore import NetworkTableInstance
 
-from constants import DriveConstants, GyroConstants
+from constants import DriveConstants
 
 import utils.swerveutils as swerveutils
 
@@ -61,18 +61,11 @@ def __init__(self) -> None:
 
         # The gyro sensor
         if wpilib.RobotBase.isReal():
-            self.gyro = Pigeon2(GyroConstants.id)
+            self.gyro = AHRS.create_spi()
         else:
             # Bug with navx init! For sim/unit testing just use the ADIS
             self.gyro = DummyGyro()
 
-        # the mounting pose for the gyro
-        self.gyro.configMountPose(
-            GyroConstants.yawPose,
-            GyroConstants.pitchPose,
-            GyroConstants.rollPose,
-        )
-
         # Slew rate filter variables for controlling lateral acceleration
         self.currentRotation = 0.0
         self.currentTranslationDir = 0.0
@@ -85,7 +78,7 @@ def __init__(self) -> None:
         # Odometry class for tracking robot pose
         self.odometry = SwerveDrive4Odometry(
             DriveConstants.kDriveKinematics,
-            Rotation2d.fromDegrees(self.gyro.getYaw()),
+            Rotation2d.fromDegrees(self.gyro.getAngle()),
             (
                 self.frontLeft.getPosition(),
                 self.frontRight.getPosition(),
@@ -97,7 +90,7 @@ def __init__(self) -> None:
     def periodic(self) -> None:
         # Update the odometry in the periodic block
         self.odometry.update(
-            Rotation2d.fromDegrees(self.gyro.getYaw()),
+            Rotation2d.fromDegrees(self.gyro.getAngle()),
             (
                 self.frontLeft.getPosition(),
                 self.frontRight.getPosition(),
@@ -140,7 +133,7 @@ def resetOdometry(self, pose: Pose2d) -> None:
 
         """
         self.odometry.resetPosition(
-            Rotation2d.fromDegrees(self.gyro.getYaw()),
+            Rotation2d.fromDegrees(self.gyro.getAngle()),
             (
                 self.frontLeft.getPosition(),
                 self.frontRight.getPosition(),
@@ -244,7 +237,7 @@ def drive(
                 xSpeedDelivered,
                 ySpeedDelivered,
                 rotDelivered,
-                Rotation2d.fromDegrees(self.gyro.getYaw()),
+                Rotation2d.fromDegrees(self.gyro.getAngle()),
             )
             if fieldRelative
             else ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)
@@ -293,16 +286,14 @@ def resetEncoders(self) -> None:
 
     def zeroHeading(self) -> None:
         """Zeroes the heading of the robot."""
-        # Pigeon2 doesn't have a reset command
-        # self.gyro.reset()
-        pass
+        self.gyro.reset()
 
     def getHeading(self) -> float:
         """Returns the heading of the robot.
 
         :returns: the robot's heading in degrees, from -180 to 180
         """
-        return Rotation2d.fromDegrees(self.gyro.getYaw()).degrees()
+        return Rotation2d.fromDegrees(self.gyro.getAngle()).degrees()
 
     def getTurnRate(self) -> float:
         """Returns the turn rate of the robot.

rio/utils/dummygyro.py

@@ -3,8 +3,5 @@ class DummyGyro:
     def __init__(self):
         pass
 
-    def getYaw(self) -> float:
+    def getAngle(self) -> float:
         return 0.0
-
-    def configMountPose(self, x, y, z):
-        pass