Upgrade to wpilib alpha-6 (#2434)

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Co-authored-by: Ryanforce08 <rradtke1208@gmail.com> Co-authored-by: PJ Reiniger <pj.reiniger@gmail.com> Co-authored-by: Jade Turner <spacey-sooty@proton.me> Co-authored-by: Matt Morley <matthew.morley.ca@gmail.com>
2026-06-19 00:41:41 +00:00 · 2026-05-26 21:47:48 -04:00
parent d3de87f72b
commit e9006a2803
97 changed files with 732 additions and 1139 deletions
--- a/photonlib-python-examples/poseest/drivetrain.py
+++ b/photonlib-python-examples/poseest/drivetrain.py
@@ -28,16 +28,14 @@ import math
 import swervemodule
 import wpilib
 import wpilib.simulation
-import wpimath.estimator
-import wpimath.geometry
-import wpimath.kinematics
+import wpimath

 kMaxSpeed = 3.0  # 3 meters per second
 kMaxAngularSpeed = math.pi  # 1/2 rotation per second

-kInitialPose = wpimath.geometry.Pose2d(
-    wpimath.geometry.Translation2d(1.0, 1.0),
-    wpimath.geometry.Rotation2d.fromDegrees(0.0),
+kInitialPose = wpimath.Pose2d(
+    wpimath.Translation2d(1.0, 1.0),
+    wpimath.Rotation2d.fromDegrees(0.0),
 )


@@ -47,10 +45,10 @@ class Drivetrain:
    """

    def __init__(self) -> None:
-        self.frontLeftLocation = wpimath.geometry.Translation2d(0.381, 0.381)
-        self.frontRightLocation = wpimath.geometry.Translation2d(0.381, -0.381)
-        self.backLeftLocation = wpimath.geometry.Translation2d(-0.381, 0.381)
-        self.backRightLocation = wpimath.geometry.Translation2d(-0.381, -0.381)
+        self.frontLeftLocation = wpimath.Translation2d(0.381, 0.381)
+        self.frontRightLocation = wpimath.Translation2d(0.381, -0.381)
+        self.backLeftLocation = wpimath.Translation2d(-0.381, 0.381)
+        self.backRightLocation = wpimath.Translation2d(-0.381, -0.381)

        self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3, 1)
        self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7, 2)
@@ -61,16 +59,17 @@ class Drivetrain:
        wpilib.SmartDashboard.putData("Drivetrain Debug", self.debugField)

        self.gyro = wpilib.AnalogGyro(0)
-        self.simGyro = wpilib.simulation.AnalogGyroSim(self.gyro)
+        self.simGyro = wpilib.simulation.OnboardIMUSim(self.gyro)
+        self._yaw = 0.0

-        self.kinematics = wpimath.kinematics.SwerveDrive4Kinematics(
+        self.kinematics = wpimath.SwerveDrive4Kinematics(
            self.frontLeftLocation,
            self.frontRightLocation,
            self.backLeftLocation,
            self.backRightLocation,
        )

-        self.poseEst = wpimath.estimator.SwerveDrive4PoseEstimator(
+        self.poseEst = wpimath.SwerveDrive4PoseEstimator(
            self.kinematics,
            self.gyro.getRotation2d(),
            (
@@ -79,10 +78,9 @@ class Drivetrain:
                self.backLeft.getPosition(),
                self.backRight.getPosition(),
            ),
-            kInitialPose,
        )

-        self.targetChassisSpeeds = wpimath.kinematics.ChassisSpeeds()
+        self.targetChassisVelocities = wpimath.ChassisVelocities()

        self.gyro.reset()

@@ -102,31 +100,28 @@ class Drivetrain:
        :param fieldRelative: Whether the provided x and y speeds are relative to the field.
        :param periodSeconds: Time
        """
-        swerveModuleStates = self.kinematics.toSwerveModuleStates(
-            wpimath.kinematics.ChassisSpeeds.discretize(
-                (
-                    wpimath.kinematics.ChassisSpeeds.fromFieldRelativeSpeeds(
-                        xSpeed, ySpeed, rot, self.gyro.getRotation2d()
-                    )
-                    if fieldRelative
-                    else wpimath.kinematics.ChassisSpeeds(xSpeed, ySpeed, rot)
-                ),
-                periodSeconds,
+        chassisVelocities = wpimath.ChassisVelocities(xSpeed, ySpeed, rot)
+        if fieldRelative:
+            chassisVelocities = chassisVelocities.toRobotRelative(
+                self.gyro.getRotation2d()
            )
-        )
-        wpimath.kinematics.SwerveDrive4Kinematics.desaturateWheelSpeeds(
-            swerveModuleStates, kMaxSpeed
-        )
-        self.frontLeft.setDesiredState(swerveModuleStates[0])
-        self.frontRight.setDesiredState(swerveModuleStates[1])
-        self.backLeft.setDesiredState(swerveModuleStates[2])
-        self.backRight.setDesiredState(swerveModuleStates[3])

-        self.targetChassisSpeeds = self.kinematics.toChassisSpeeds(swerveModuleStates)
+        chassisVelocities = chassisVelocities.discretize(periodSeconds)
+        swerveModuleStates = self.kinematics.desaturateWheelVelocities(
+            self.kinematics.toSwerveModuleVelocities(chassisVelocities), kMaxSpeed
+        )
+        self.frontLeft.setDesiredVelocity(swerveModuleStates[0])
+        self.frontRight.setDesiredVelocity(swerveModuleStates[1])
+        self.backLeft.setDesiredVelocity(swerveModuleStates[2])
+        self.backRight.setDesiredVelocity(swerveModuleStates[3])
+
+        self.targetChassisVelocities = self.kinematics.toChassisVelocities(
+            swerveModuleStates
+        )

    def updateOdometry(self) -> None:
        """Updates the field relative position of the robot."""
-        self.poseEst.update(
+        self.odometry.update(
            self.gyro.getRotation2d(),
            (
                self.frontLeft.getPosition(),
@@ -136,9 +131,7 @@ class Drivetrain:
            ),
        )

-    def addVisionPoseEstimate(
-        self, pose: wpimath.geometry.Pose3d, timestamp: float
-    ) -> None:
+    def addVisionPoseEstimate(self, pose: wpimath.Pose3d, timestamp: float) -> None:
        self.poseEst.addVisionMeasurement(pose, timestamp)

    def resetPose(self) -> None:
@@ -153,26 +146,26 @@ class Drivetrain:
            kInitialPose,
        )

-    def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]:
+    def getModuleVelocities(self) -> list[wpimath.SwerveModuleVelocity]:
        return [
-            self.frontLeft.getState(),
-            self.frontRight.getState(),
-            self.backLeft.getState(),
-            self.backRight.getState(),
+            self.frontLeft.getVelocity(),
+            self.frontRight.getVelocity(),
+            self.backLeft.getVelocity(),
+            self.backRight.getVelocity(),
        ]

-    def getModulePoses(self) -> list[wpimath.geometry.Pose2d]:
+    def getModulePoses(self) -> list[wpimath.Pose2d]:
        p = self.poseEst.getEstimatedPosition()
-        flTrans = wpimath.geometry.Transform2d(
+        flTrans = wpimath.Transform2d(
            self.frontLeftLocation, self.frontLeft.getAbsoluteHeading()
        )
-        frTrans = wpimath.geometry.Transform2d(
+        frTrans = wpimath.Transform2d(
            self.frontRightLocation, self.frontRight.getAbsoluteHeading()
        )
-        blTrans = wpimath.geometry.Transform2d(
+        blTrans = wpimath.Transform2d(
            self.backLeftLocation, self.backLeft.getAbsoluteHeading()
        )
-        brTrans = wpimath.geometry.Transform2d(
+        brTrans = wpimath.Transform2d(
            self.backRightLocation, self.backRight.getAbsoluteHeading()
        )
        return [
@@ -182,18 +175,18 @@ class Drivetrain:
            p.transformBy(brTrans),
        ]

-    def getChassisSpeeds(self) -> wpimath.kinematics.ChassisSpeeds:
-        return self.kinematics.toChassisSpeeds(self.getModuleStates())
+    def getChassisVelocities(self) -> wpimath.ChassisVelocities:
+        return self.kinematics.toChassisVelocities(self.getModuleVelocities())

    def log(self):
        table = "Drive/"

-        pose = self.poseEst.getEstimatedPosition()
+        pose = self.odometry.getPose()
        wpilib.SmartDashboard.putNumber(table + "X", pose.X())
        wpilib.SmartDashboard.putNumber(table + "Y", pose.Y())
        wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees())

-        chassisSpeeds = self.getChassisSpeeds()
+        chassisSpeeds = self.getChassisVelocities()
        wpilib.SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx)
        wpilib.SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy)
        wpilib.SmartDashboard.putNumber(
@@ -201,13 +194,13 @@ class Drivetrain:
        )

        wpilib.SmartDashboard.putNumber(
-            table + "Target VX", self.targetChassisSpeeds.vx
+            table + "Target VX", self.targetChassisVelocities.vx
        )
        wpilib.SmartDashboard.putNumber(
-            table + "Target VY", self.targetChassisSpeeds.vy
+            table + "Target VY", self.targetChassisVelocities.vy
        )
        wpilib.SmartDashboard.putNumber(
-            table + "Target Omega Degrees", self.targetChassisSpeeds.omega_dps
+            table + "Target Omega Degrees", self.targetChassisVelocities.omega_dps
        )

        self.frontLeft.log()
@@ -215,7 +208,7 @@ class Drivetrain:
        self.backLeft.log()
        self.backRight.log()

-        self.debugField.getRobotObject().setPose(self.poseEst.getEstimatedPosition())
+        self.debugField.getRobotObject().setPose(self.odometry.getPose())
        self.debugField.getObject("SwerveModules").setPoses(self.getModulePoses())

    def simulationPeriodic(self):
@@ -223,5 +216,7 @@ class Drivetrain:
        self.frontRight.simulationPeriodic()
        self.backLeft.simulationPeriodic()
        self.backRight.simulationPeriodic()
-        self.simGyro.setRate(-1.0 * self.getChassisSpeeds().omega_dps)
-        self.simGyro.setAngle(self.simGyro.getAngle() + self.simGyro.getRate() * 0.02)
+        rate = -1.0 * self.getChassisVelocities().omega_dps
+        self.simGyro.setRate(rate)
+        self._yaw += rate * 0.02
+        self.simGyro.setYaw(self._yaw)
--- a/photonlib-python-examples/poseest/robot.py
+++ b/photonlib-python-examples/poseest/robot.py
@@ -26,20 +26,22 @@

 import drivetrain
 import wpilib
-import wpimath.geometry
+import wpimath
 from photonlibpy import PhotonCamera, PhotonPoseEstimator
 from robotpy_apriltag import AprilTagField, AprilTagFieldLayout

-kRobotToCam = wpimath.geometry.Transform3d(
-    wpimath.geometry.Translation3d(0.5, 0.0, 0.5),
-    wpimath.geometry.Rotation3d.fromDegrees(0.0, -30.0, 0.0),
+kRobotToCam = wpimath.Transform3d(
+    wpimath.Translation3d(0.5, 0.0, 0.5),
+    wpimath.Rotation3d.fromDegrees(0.0, -30.0, 0.0),
 )


 class MyRobot(wpilib.TimedRobot):
-    def robotInit(self) -> None:
+    def __init__(self) -> None:
        """Robot initialization function"""
-        self.controller = wpilib.XboxController(0)
+        super().__init__()
+
+        self.controller = wpilib.NiDsXboxController(0)
        self.swerve = drivetrain.Drivetrain()
        self.cam = PhotonCamera("YOUR CAMERA NAME")
        self.camPoseEst = PhotonPoseEstimator(
--- a/photonlib-python-examples/poseest/swervemodule.py
+++ b/photonlib-python-examples/poseest/swervemodule.py
@@ -1,4 +1,4 @@
-###################################################################################
+Speeds  ###################################################################################
 # MIT License
 #
 # Copyright (c) PhotonVision
@@ -26,11 +26,7 @@ import math

 import wpilib
 import wpilib.simulation
-import wpimath.controller
-import wpimath.filter
-import wpimath.geometry
-import wpimath.kinematics
-import wpimath.trajectory
+import wpimath
 import wpimath.units

 kWheelRadius = 0.0508
@@ -60,7 +56,7 @@ class SwerveModule:
        :param turningEncoderChannelB: DIO input for the turning encoder channel B
        """
        self.moduleNumber = moduleNumber
-        self.desiredState = wpimath.kinematics.SwerveModuleState()
+        self.desiredVelocity = wpimath.SwerveModuleVelocity()

        self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel)
        self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel)
@@ -71,13 +67,14 @@ class SwerveModule:
        )

        # Gains are for example purposes only - must be determined for your own robot!
-        self.drivePIDController = wpimath.controller.PIDController(10, 0, 0)
+        self.drivePIDController = wpimath.PIDController(10, 0, 0)

        # Gains are for example purposes only - must be determined for your own robot!
-        self.turningPIDController = wpimath.controller.PIDController(30, 0, 0)
+        self.turningPIDController = wpimath.PIDController(30, 0, 0)

        # Gains are for example purposes only - must be determined for your own robot!
-        self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3)
+        self.driveFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3)
+        self.turnFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 0.7)

        # Set the distance per pulse for the drive encoder. We can simply use the
        # distance traveled for one rotation of the wheel divided by the encoder
@@ -98,76 +95,74 @@ class SwerveModule:
        # Simulation Support
        self.simDriveEncoder = wpilib.simulation.EncoderSim(self.driveEncoder)
        self.simTurningEncoder = wpilib.simulation.EncoderSim(self.turningEncoder)
-        self.simDrivingMotor = wpilib.simulation.PWMSim(self.driveMotor)
-        self.simTurningMotor = wpilib.simulation.PWMSim(self.turningMotor)
-        self.simDrivingMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(
-            0.1, 0.02
-        )
-        self.simTurningMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(
-            0.0001, 0.02
-        )
+        self.simDrivingMotor = wpilib.simulation.PWMSim(driveMotorChannel)
+        self.simTurningMotor = wpilib.simulation.PWMSim(turningMotorChannel)
+        self.simDrivingMotorFilter = wpimath.LinearFilter.singlePoleIIR(0.1, 0.02)
+        self.simTurningMotorFilter = wpimath.LinearFilter.singlePoleIIR(0.0001, 0.02)
        self.simTurningEncoderPos = 0
        self.simDrivingEncoderPos = 0

-    def getState(self) -> wpimath.kinematics.SwerveModuleState:
+    def getVelocity(self) -> wpimath.SwerveModuleVelocity:
        """Returns the current state of the module.

        :returns: The current state of the module.
        """
-        return wpimath.kinematics.SwerveModuleState(
+        return wpimath.SwerveModuleVelocity(
            self.driveEncoder.getRate(),
-            wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()),
+            wpimath.Rotation2d(self.turningEncoder.getDistance()),
        )

-    def getPosition(self) -> wpimath.kinematics.SwerveModulePosition:
+    def getPosition(self) -> wpimath.SwerveModulePosition:
        """Returns the current position of the module.

        :returns: The current position of the module.
        """
-        return wpimath.kinematics.SwerveModulePosition(
+        return wpimath.SwerveModulePosition(
            self.driveEncoder.getDistance(),
-            wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()),
+            wpimath.Rotation2d(self.turningEncoder.getDistance()),
        )

-    def setDesiredState(
-        self, desiredState: wpimath.kinematics.SwerveModuleState
-    ) -> None:
+    def setDesiredVelocity(self, desiredVelocity: wpimath.SwerveModuleVelocity) -> None:
        """Sets the desired state for the module.

-        :param desiredState: Desired state with speed and angle.
+        :param desiredVelocity: Desired state with speed and angle.
        """
-        self.desiredState = desiredState
+        self.desiredVelocity = desiredVelocity

-        encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getDistance())
+        encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance())

        # Optimize the reference state to avoid spinning further than 90 degrees
-        self.desiredState.optimize(encoderRotation)
+        self.desiredVelocity.optimize(encoderRotation)

        # Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
        # direction of travel that can occur when modules change directions. This results in smoother
        # driving.
-        self.desiredState.speed *= (self.desiredState.angle - encoderRotation).cos()
+        self.desiredVelocity.velocity *= (
+            self.desiredVelocity.angle - encoderRotation
+        ).cos()

        # Calculate the drive output from the drive PID controller.
        driveOutput = self.drivePIDController.calculate(
-            self.driveEncoder.getRate(), self.desiredState.speed
+            self.driveEncoder.getRate(), self.desiredVelocity.velocity
        )

-        driveFeedforward = self.driveFeedforward.calculate(self.desiredState.speed)
+        driveFeedforward = self.driveFeedforward.calculate(
+            self.desiredVelocity.velocity
+        )

        # Calculate the turning motor output from the turning PID controller.
        turnOutput = self.turningPIDController.calculate(
-            self.turningEncoder.getDistance(), self.desiredState.angle.radians()
+            self.turningEncoder.getDistance(), self.desiredVelocity.angle.radians()
        )

        self.driveMotor.setVoltage(driveOutput + driveFeedforward)
-        self.turningMotor.setVoltage(turnOutput)
+        self.turningMotor.setVoltage(turnOutput + turnFeedforward)

-    def getAbsoluteHeading(self) -> wpimath.geometry.Rotation2d:
-        return wpimath.geometry.Rotation2d(self.turningEncoder.getDistance())
+    def getAbsoluteHeading(self) -> wpimath.Rotation2d:
+        return wpimath.Rotation2d(self.turningEncoder.getDistance())

    def log(self) -> None:
-        state = self.getState()
+        state = self.getVelocity()

        table = "Module " + str(self.moduleNumber) + "/"
        wpilib.SmartDashboard.putNumber(
@@ -178,15 +173,17 @@ class SwerveModule:
            table + "Steer Target Degrees",
            math.degrees(self.turningPIDController.getSetpoint()),
        )
-        wpilib.SmartDashboard.putNumber(table + "Drive Velocity Feet", state.speed_fps)
        wpilib.SmartDashboard.putNumber(
-            table + "Drive Velocity Target Feet", self.desiredState.speed_fps
+            table + "Drive Velocity Feet", state.velocity_fps
        )
        wpilib.SmartDashboard.putNumber(
-            table + "Drive Voltage", self.driveMotor.get() * 12.0
+            table + "Drive Velocity Target Feet", self.desiredVelocity.velocity_fps
        )
        wpilib.SmartDashboard.putNumber(
-            table + "Steer Voltage", self.turningMotor.get() * 12.0
+            table + "Drive Throttle", self.driveMotor.getThrottle() * 12.0
+        )
+        wpilib.SmartDashboard.putNumber(
+            table + "Steer Throttle", self.turningMotor.getThrottle() * 12.0
        )

    def simulationPeriodic(self) -> None:
@@ -199,7 +196,9 @@ class SwerveModule:
        driveSpdRaw = (
            driveVoltage / 12.0 * self.driveFeedforward.maxAchievableVelocity(12.0, 0)
        )
-        turnSpdRaw = turnVoltage / 0.7
+        turnSpdRaw = (
+            turnVoltage / 12.0 * self.turnFeedforward.maxAchievableVelocity(12.0, 0)
+        )
        driveSpd = self.simDrivingMotorFilter.calculate(driveSpdRaw)
        turnSpd = self.simTurningMotorFilter.calculate(turnSpdRaw)
        self.simDrivingEncoderPos += 0.02 * driveSpd