Update to RobotPy 2023 beta

Pipfile

@@ -4,24 +4,24 @@ verify_ssl = true
 name = "pypi"
 
 [packages]
-pyfrc = "~=2022.1.2"
-robotpy-halsim-gui = "~=2022.4.1"
-numpy = "~=1.22.1"
-robotpy-ctre = "~=2022.1.0"
-robotpy-navx = "~=2022.0.1"
-robotpy-photonvision = "~=2022.1.0"
-robotpy-rev = "~=2022.0.1"
-robotpy-wpilib-utilities = "~=2022.0.5"
-robotpy-wpimath = "~=2022.4.1.1"
-robotpy-wpiutil = "~=2022.4.1.1"
-wpilib = "~=2022.4.1.5"
+pyfrc = "==2023.0.0b4"
+robotpy-halsim-gui = "==2023.0.0b7"
+numpy = "==1.24.1"
+robotpy-ctre = "==2023.0.0b6"
+robotpy-navx = "==2023.0.0b2"
+robotpy-photonvision = "==2023.1.1b7"
+robotpy-rev = "==2023.0.0b2"
+robotpy-wpilib-utilities = "==2023.0.0b2"
+robotpy-wpimath = "==2023.0.0b7"
+robotpy-wpiutil = "==2023.0.0b7"
+wpilib = "==2023.0.0b7"
 
 [dev-packages]
 hypothesis = "*"
 pytest = "*"
 
 [requires]
-python_version = "3.10"
+python_version = "3.11"
 
 [scripts]
 deploy = "python robot.py deploy"

autonomous/autonomous.py

@@ -148,7 +148,7 @@ def move(self, tm: float) -> None:
         self.chassis_speeds = self.drive_controller.calculate(
             currentPose=current_pose,
             desiredState=target_state,
-            angleRef=target_heading,
+            desiredHeading=target_heading,
         )
         self.chassis.drive_local(
             self.chassis_speeds.vx, self.chassis_speeds.vy, self.chassis_speeds.omega

components/chassis.py

@@ -11,6 +11,7 @@
 from wpimath.kinematics import (
     SwerveDrive4Kinematics,
     ChassisSpeeds,
+    SwerveModulePosition,
     SwerveModuleState,
 )
 from wpimath.geometry import Translation2d, Rotation2d, Pose2d
@@ -103,6 +104,9 @@ def get_rotation(self) -> Rotation2d:
     def get_speed(self) -> float:
         return self.drive.getSelectedSensorVelocity() * self.DRIVE_COUNTS_TO_METRES * 10
 
+    def get_distance_travelled(self) -> float:
+        return self.drive.getSelectedSensorPosition() * self.DRIVE_COUNTS_TO_METRES
+
     def set(self, desired_state: SwerveModuleState):
 
         if abs(desired_state.speed) < 1e-3:
@@ -134,6 +138,9 @@ def sync_steer_encoders(self) -> None:
             self.get_absolute_angle() * self.STEER_RAD_TO_COUNTS
         )
 
+    def get_position(self) -> SwerveModulePosition:
+        return SwerveModulePosition(self.get_distance_travelled(), self.get_rotation())
+
     def get(self) -> SwerveModuleState:
         return SwerveModuleState(self.get_speed(), self.get_rotation())
 
@@ -228,11 +235,11 @@ def setup(self) -> None:
         self.sync_all()
         self.imu.zeroYaw()
         self.estimator = SwerveDrive4PoseEstimator(
-            self.imu.getRotation2d(),
-            Pose2d(0, 0, 0),
             self.kinematics,
+            self._get_imu_heading(),
+            self.get_module_positions(),
+            Pose2d(0, 0, 0),
             stateStdDevs=(0.1, 0.1, math.radians(5)),
-            localMeasurementStdDevs=(0.01,),
             visionMeasurementStdDevs=(0.5, 0.5, 0.2),
         )
         self.field_obj = self.field.getObject("fused_pose")
@@ -265,12 +272,10 @@ def execute(self) -> None:
         # rotation2d and translation2d have mul but not div
         control_rate = 1 / dt
         chassis_speeds = self.kinematics.toChassisSpeeds(
-            (
-                self.modules[0].get(),
-                self.modules[1].get(),
-                self.modules[2].get(),
-                self.modules[3].get(),
-            )
+            self.modules[0].get(),
+            self.modules[1].get(),
+            self.modules[2].get(),
+            self.modules[3].get(),
         )
         cur_trans_vel = Translation2d(chassis_speeds.vx, chassis_speeds.vy).rotateBy(
             self.get_rotation()
@@ -299,7 +304,9 @@ def sync_all(self) -> None:
 
     def set_pose(self, pose: Pose2d) -> None:
         self.pose_history.clear()
-        self.estimator.resetPosition(pose, self.imu.getRotation2d())
+        self.estimator.resetPosition(
+            self._get_imu_heading(), self.get_module_positions(), pose
+        )
         self.update_pose_history()
         self.field.setRobotPose(pose)
         self.field_obj.setPose(pose)
@@ -321,9 +328,14 @@ def zero_yaw(self) -> None:
         # a misake copied from diff drive pose estimator
         # beacuse we never pass the encoder distances to the estimator
         self.estimator.resetPosition(
-            Pose2d(cur_pose.translation(), Rotation2d(0)), self.imu.getRotation2d()
+            self._get_imu_heading(),
+            self.get_module_positions(),
+            Pose2d(cur_pose.translation(), Rotation2d(0)),
         )
 
+    def get_module_positions(self):
+        return tuple(module.get_position() for module in self.modules)
+
     def get_pose(self) -> Pose2d:
         """Get the current location of the robot relative to the goal."""
         return self.estimator.getEstimatedPosition()
@@ -332,6 +344,10 @@ def get_rotation(self) -> Rotation2d:
         """Get the current heading of the robot."""
         return self.get_pose().rotation()
 
+    def _get_imu_heading(self) -> Rotation2d:
+        """Get the heading from the IMU."""
+        return self.imu.getRotation2d()
+
     def get_pose_at(self, t: float) -> Pose2d:
         """Gets where the robot was at t"""
         return self.pose_history.sample(t)
@@ -349,11 +365,8 @@ def robot_to_world(
 
     def update_odometry(self) -> None:
         self.estimator.update(
-            self.imu.getRotation2d(),
-            self.modules[0].get(),
-            self.modules[1].get(),
-            self.modules[2].get(),
-            self.modules[3].get(),
+            self._get_imu_heading(),
+            self.get_module_positions(),
         )
         self.field_obj.setPose(self.get_pose())
 

components/vision.py

@@ -13,8 +13,6 @@
 )
 from wpimath.geometry import Pose2d, Transform2d, Translation2d, Rotation2d
 
-import random
-
 
 class Vision:
     """Communicates with limelight to get vision data and calculate pose"""
@@ -48,6 +46,7 @@ def _camera_to_robot(self) -> Transform2d:
     def __init__(self) -> None:
         self.camera = PhotonCamera("gloworm")
 
+        """
         if wpilib.RobotBase.isSimulation():
             self.sim_vision_system = SimVisionSystem(
                 "gloworm",
@@ -69,6 +68,7 @@ def __init__(self) -> None:
                 )
             )
             self.camera = self.sim_vision_system.cam
+        """
 
         self.camera.setLEDMode(LEDMode.kOn)
         self.max_std_dev = 0.05
@@ -83,6 +83,7 @@ def setup(self) -> None:
         self.field_obj = self.field.getObject("vision_pose")
 
     def execute(self) -> None:
+        """
         if wpilib.RobotBase.isSimulation():
             # Create some vision target results
             pose = self.field.getRobotPose()
@@ -106,6 +107,7 @@ def execute(self) -> None:
                 newCamPitch=math.degrees(self.CAMERA_PITCH) + random.gauss(0.0, 0.5),
             )
             self.sim_vision_system.processFrame(pose)
+        """
 
         results = self.camera.getLatestResult()
         self.has_target = results.hasTargets()

physics.py

@@ -120,7 +120,7 @@ def update_sim(self, now: float, tm_diff: float) -> None:
             ],
             tuple([module.get() for module in self.swerve_modules]),
         )
-        speeds = self.kinematics.toChassisSpeeds(states)
+        speeds = self.kinematics.toChassisSpeeds(*states)
 
         self.imu_yaw.set(self.imu_yaw.get() - math.degrees(speeds.omega * tm_diff))
 

requirements-sim.txt

@@ -1,3 +1,3 @@
-pyfrc ~= 2022.1.2
-robotpy-halsim-gui ~= 2022.4.1
+pyfrc==2023.0.0b4
+robotpy-halsim-gui==2023.0.0b7
 -r requirements.txt

requirements-test.txt

@@ -1,4 +1,4 @@
 hypothesis
-pyfrc ~= 2022.1.2
+pyfrc==2023.0.0b4
 pytest
 -r requirements.txt

requirements.txt

@@ -1,9 +1,9 @@
-numpy ~= 1.22.1
-robotpy-ctre ~= 2022.1.0
-robotpy-navx ~= 2022.0.1
-robotpy-rev ~= 2022.0.1
-robotpy-wpilib-utilities ~= 2022.0.5
-robotpy-wpimath ~= 2022.4.1.1
-robotpy-wpiutil ~= 2022.4.1.1
-wpilib ~= 2022.4.1.5
-robotpy-photonvision ~= 2022.1.0
+numpy==1.24.1
+robotpy-ctre==2023.0.0b6
+robotpy-navx==2023.0.0b2
+robotpy-rev==2023.0.0b2
+robotpy-wpilib-utilities==2023.0.0b2
+robotpy-wpimath==2023.0.0b7
+robotpy-wpiutil==2023.0.0b7
+wpilib==2023.0.0b7
+robotpy-photonvision==2023.1.1b7

robot.py

@@ -113,7 +113,7 @@ def createObjects(self) -> None:
             except ImportError:
                 self.logger.exception("Could not import CameraServer")
             else:
-                CameraServer.getInstance().startAutomaticCapture()
+                CameraServer.startAutomaticCapture()
 
     def autonomousInit(self) -> None:
         self.shooter_control.lead_shots = False

tests/pyfrc_test.py

@@ -1,14 +1,13 @@
 # NB: autonomous is tested separately in autonomous_test.py
 from pyfrc.tests import (  # type: ignore
-    # Running all the tests seems to segfault, but a subset seems okay?
-    # test_disabled,
-    # test_operator_control,
+    test_disabled,
+    test_operator_control,
     test_practice,
 )
 
 # Make pyflakes happy about our imports.
 __all__ = (
-    # "test_disabled",
-    # "test_operator_control",
+    "test_disabled",
+    "test_operator_control",
     "test_practice",
 )