[robotpy][examples] Split examples and snippets (#8944)

This also updates the bazel scripts to behave more like the C++ and Java
examples, and updates the copybara scripts to be able to sync up
`mostrobotpy`

robotpyExamples/examples/ArcadeDrive/robot.py

@@ -0,0 +1,35 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    This is a demo program showing the use of the DifferentialDrive class.
+    Runs the motors with arcade steering.
+    """
+
+    def __init__(self):
+        """Robot initialization function"""
+        super().__init__()
+
+        leftMotor = wpilib.PWMSparkMax(0)
+        rightMotor = wpilib.PWMSparkMax(1)
+        self.robotDrive = wpilib.DifferentialDrive(leftMotor, rightMotor)
+        self.stick = wpilib.Joystick(0)
+
+        # We need to invert one side of the drivetrain so that positive voltages
+        # result in both sides moving forward. Depending on how your robot's
+        # gearbox is constructed, you might have to invert the left side instead.
+        rightMotor.setInverted(True)
+
+    def teleopPeriodic(self):
+        # Drive with arcade drive.
+        # That means that the Y axis drives forward
+        # and backward, and the X turns left and right.
+        self.robotDrive.arcadeDrive(-self.stick.getY(), -self.stick.getX())

robotpyExamples/examples/ArcadeDriveXboxController/robot.py

@@ -0,0 +1,37 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    This is a demo program showing the use of the DifferentialDrive class.
+    Runs the motors with split arcade steering and an Xbox controller.
+    """
+
+    def __init__(self):
+        """Robot initialization function"""
+        super().__init__()
+
+        leftMotor = wpilib.PWMSparkMax(0)
+        rightMotor = wpilib.PWMSparkMax(1)
+        self.robotDrive = wpilib.DifferentialDrive(leftMotor, rightMotor)
+        self.driverController = wpilib.NiDsXboxController(0)
+
+        # We need to invert one side of the drivetrain so that positive voltages
+        # result in both sides moving forward. Depending on how your robot's
+        # gearbox is constructed, you might have to invert the left side instead.
+        rightMotor.setInverted(True)
+
+    def teleopPeriodic(self):
+        # Drive with split arcade style
+        # That means that the Y axis of the left stick moves forward
+        # and backward, and the X of the right stick turns left and right.
+        self.robotDrive.arcadeDrive(
+            -self.driverController.getLeftY(), -self.driverController.getRightX()
+        )

robotpyExamples/examples/ArmSimulation/constants.py

@@ -0,0 +1,33 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+
+from wpimath import units
+
+
+class Constants:
+    kMotorPort = 0
+    kEncoderAChannel = 0
+    kEncoderBChannel = 1
+    kJoystickPort = 0
+
+    kArmPositionKey = "ArmPosition"
+    kArmPKey = "ArmP"
+
+    # The P gain for the PID controller that drives this arm.
+    kDefaultArmKp = 50.0
+    kDefaultArmSetpointDegrees = 75.0
+
+    # distance per pulse = (angle per revolution) / (pulses per revolution)
+    #  = (2 * PI rads) / (4096 pulses)
+    kArmEncoderDistPerPulse = 2.0 * math.pi / 4096
+
+    kArmReduction = 200
+    kArmMass = 8.0  # Kilograms
+    kArmLength = units.inchesToMeters(30)
+    kMinAngleRads = units.degreesToRadians(-75)
+    kMaxAngleRads = units.degreesToRadians(255)

robotpyExamples/examples/ArmSimulation/robot.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+
+from constants import Constants
+from subsystems.arm import Arm
+
+
+class MyRobot(wpilib.TimedRobot):
+    """This is a sample program to demonstrate the use of arm simulation with existing code."""
+
+    def __init__(self) -> None:
+        super().__init__()
+        self.arm = Arm()
+        self.joystick = wpilib.Joystick(Constants.kJoystickPort)
+
+    def simulationPeriodic(self) -> None:
+        self.arm.simulationPeriodic()
+
+    def teleopInit(self) -> None:
+        self.arm.loadPreferences()
+
+    def teleopPeriodic(self) -> None:
+        if self.joystick.getTrigger():
+            # Here, we run PID control like normal.
+            self.arm.reachSetpoint()
+        else:
+            # Otherwise, we disable the motor.
+            self.arm.stop()
+
+    def disabledInit(self) -> None:
+        # This just makes sure that our simulation code knows that the motor's off.
+        self.arm.stop()

robotpyExamples/examples/ArmSimulation/subsystems/arm.py

@@ -0,0 +1,114 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import wpilib.simulation
+import wpimath
+import wpimath.units
+import wpiutil
+
+from constants import Constants
+
+
+class Arm:
+    def __init__(self) -> None:
+        # The P gain for the PID controller that drives this arm.
+        self.armKp = Constants.kDefaultArmKp
+        self.armSetpointDegrees = Constants.kDefaultArmSetpointDegrees
+
+        # The arm gearbox represents a gearbox containing two Vex 775pro motors.
+        self.armGearbox = wpimath.DCMotor.vex775Pro(2)
+
+        # Standard classes for controlling our arm
+        self.controller = wpimath.PIDController(self.armKp, 0, 0)
+        self.encoder = wpilib.Encoder(
+            Constants.kEncoderAChannel, Constants.kEncoderBChannel
+        )
+        self.motor = wpilib.PWMSparkMax(Constants.kMotorPort)
+
+        # Simulation classes help us simulate what's going on, including gravity.
+        # This arm sim represents an arm that can travel from -75 degrees (rotated down front)
+        # to 255 degrees (rotated down in the back).
+        self.armSim = wpilib.simulation.SingleJointedArmSim(
+            self.armGearbox,
+            Constants.kArmReduction,
+            wpilib.simulation.SingleJointedArmSim.estimateMOI(
+                Constants.kArmLength, Constants.kArmMass
+            ),
+            Constants.kArmLength,
+            Constants.kMinAngleRads,
+            Constants.kMaxAngleRads,
+            True,
+            0,
+            [Constants.kArmEncoderDistPerPulse, 0.0],
+        )
+        self.encoderSim = wpilib.simulation.EncoderSim(self.encoder)
+
+        # Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm.
+        self.mech2d = wpilib.Mechanism2d(60, 60)
+        self.armPivot = self.mech2d.getRoot("ArmPivot", 30, 30)
+        self.armTower = self.armPivot.appendLigament("ArmTower", 30, -90)
+        self.armLigament = self.armPivot.appendLigament(
+            "Arm",
+            30,
+            wpimath.units.radiansToDegrees(self.armSim.getAngle()),
+            6,
+            wpiutil.Color8Bit(wpiutil.Color.YELLOW),
+        )
+
+        # Subsystem constructor.
+        self.encoder.setDistancePerPulse(Constants.kArmEncoderDistPerPulse)
+
+        # Put Mechanism 2d to SmartDashboard
+        wpilib.SmartDashboard.putData("Arm Sim", self.mech2d)
+        self.armTower.setColor(wpiutil.Color8Bit(wpiutil.Color.BLUE))
+
+        # Set the Arm position setpoint and P constant to Preferences if the keys don't already exist
+        wpilib.Preferences.initDouble(
+            Constants.kArmPositionKey, self.armSetpointDegrees
+        )
+        wpilib.Preferences.initDouble(Constants.kArmPKey, self.armKp)
+
+    def simulationPeriodic(self) -> None:
+        # In this method, we update our simulation of what our arm is doing
+        # First, we set our "inputs" (voltages)
+        self.armSim.setInput(
+            [self.motor.getThrottle() * wpilib.RobotController.getBatteryVoltage()]
+        )
+
+        # Next, we update it. The standard loop time is 20ms.
+        self.armSim.update(0.020)
+
+        # Finally, we set our simulated encoder's readings and simulated battery voltage
+        self.encoderSim.setDistance(self.armSim.getAngle())
+        # SimBattery estimates loaded battery voltages
+        wpilib.simulation.RoboRioSim.setVInVoltage(
+            wpilib.simulation.BatterySim.calculate([self.armSim.getCurrentDraw()])
+        )
+
+        # Update the Mechanism Arm angle based on the simulated arm angle
+        self.armLigament.setAngle(
+            wpimath.units.radiansToDegrees(self.armSim.getAngle())
+        )
+
+    def loadPreferences(self) -> None:
+        # Read Preferences for Arm setpoint and kP on entering Teleop
+        self.armSetpointDegrees = wpilib.Preferences.getDouble(
+            Constants.kArmPositionKey, self.armSetpointDegrees
+        )
+        if self.armKp != wpilib.Preferences.getDouble(Constants.kArmPKey, self.armKp):
+            self.armKp = wpilib.Preferences.getDouble(Constants.kArmPKey, self.armKp)
+            self.controller.setP(self.armKp)
+
+    def reachSetpoint(self) -> None:
+        pidOutput = self.controller.calculate(
+            self.encoder.getDistance(),
+            wpimath.units.degreesToRadians(self.armSetpointDegrees),
+        )
+        self.motor.setVoltage(pidOutput)
+
+    def stop(self) -> None:
+        self.motor.setThrottle(0.0)

robotpyExamples/examples/DifferentialDriveBot/drivetrain.py

@@ -0,0 +1,109 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+import math
+
+import wpilib
+import wpimath
+
+
+class Drivetrain:
+    """Represents a differential drive style drivetrain."""
+
+    kMaxVelocity = 3.0  # meters per second
+    kMaxAngularVelocity = 2 * math.pi  # one rotation per second
+
+    kTrackwidth = 0.381 * 2  # meters
+    kWheelRadius = 0.0508  # meters
+    kEncoderResolution = 4096  # counts per revolution
+
+    def __init__(self) -> None:
+        self.leftLeader = wpilib.PWMSparkMax(1)
+        self.leftFollower = wpilib.PWMSparkMax(2)
+        self.rightLeader = wpilib.PWMSparkMax(3)
+        self.rightFollower = wpilib.PWMSparkMax(4)
+
+        # Make sure both motors for each side are in the same group
+        self.leftLeader.addFollower(self.leftFollower)
+        self.rightLeader.addFollower(self.rightFollower)
+
+        # We need to invert one side of the drivetrain so that positive voltages
+        # result in both sides moving forward. Depending on how your robot's
+        # gearbox is constructed, you might have to invert the left side instead.
+        self.rightLeader.setInverted(True)
+
+        self.leftEncoder = wpilib.Encoder(0, 1)
+        self.rightEncoder = wpilib.Encoder(2, 3)
+
+        self.imu = wpilib.OnboardIMU(wpilib.OnboardIMU.MountOrientation.FLAT)
+
+        self.leftPIDController = wpimath.PIDController(1.0, 0.0, 0.0)
+        self.rightPIDController = wpimath.PIDController(1.0, 0.0, 0.0)
+
+        self.kinematics = wpimath.DifferentialDriveKinematics(self.kTrackwidth)
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.feedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3)
+
+        self.imu.resetYaw()
+
+        # Set the distance per pulse for the drive encoders. We can simply use the
+        # distance traveled for one rotation of the wheel divided by the encoder
+        # resolution.
+        self.leftEncoder.setDistancePerPulse(
+            2 * math.pi * self.kWheelRadius / self.kEncoderResolution
+        )
+        self.rightEncoder.setDistancePerPulse(
+            2 * math.pi * self.kWheelRadius / self.kEncoderResolution
+        )
+
+        self.leftEncoder.reset()
+        self.rightEncoder.reset()
+
+        self.odometry = wpimath.DifferentialDriveOdometry(
+            self.imu.getRotation2d(),
+            self.leftEncoder.getDistance(),
+            self.rightEncoder.getDistance(),
+        )
+
+    def setVelocities(
+        self, velocities: wpimath.DifferentialDriveWheelVelocities
+    ) -> None:
+        """Sets the desired wheel velocities.
+
+        :param velocities: The desired wheel velocities.
+        """
+        leftFeedforward = self.feedforward.calculate(velocities.left)
+        rightFeedforward = self.feedforward.calculate(velocities.right)
+
+        leftOutput = self.leftPIDController.calculate(
+            self.leftEncoder.getRate(), velocities.left
+        )
+        rightOutput = self.rightPIDController.calculate(
+            self.rightEncoder.getRate(), velocities.right
+        )
+
+        # Controls the left and right sides of the robot using the calculated outputs
+        self.leftLeader.setVoltage(leftOutput + leftFeedforward)
+        self.rightLeader.setVoltage(rightOutput + rightFeedforward)
+
+    def drive(self, xVelocity: float, rot: float) -> None:
+        """Drives the robot with the given linear velocity and angular velocity.
+
+        :param xVelocity: Linear velocity in m/s.
+        :param rot: Angular velocity in rad/s.
+        """
+        wheelVelocities = self.kinematics.toWheelVelocities(
+            wpimath.ChassisVelocities(xVelocity, 0.0, rot)
+        )
+        self.setVelocities(wheelVelocities)
+
+    def updateOdometry(self) -> None:
+        """Updates the field-relative position."""
+        self.odometry.update(
+            self.imu.getRotation2d(),
+            self.leftEncoder.getDistance(),
+            self.rightEncoder.getDistance(),
+        )

robotpyExamples/examples/DifferentialDriveBot/robot.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpimath
+import wpilib
+
+from drivetrain import Drivetrain
+
+
+class MyRobot(wpilib.TimedRobot):
+    def __init__(self) -> None:
+        super().__init__()
+
+        self.controller = wpilib.NiDsXboxController(0)
+        self.drive = Drivetrain()
+
+        # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+        self.velocityLimiter = wpimath.SlewRateLimiter(3)
+        self.rotLimiter = wpimath.SlewRateLimiter(3)
+
+    def autonomousPeriodic(self) -> None:
+        self.teleopPeriodic()
+        self.drive.updateOdometry()
+
+    def teleopPeriodic(self) -> None:
+        # Get the x velocity. We are inverting this because Xbox controllers return
+        # negative values when we push forward.
+        xVelocity = (
+            -self.velocityLimiter.calculate(self.controller.getLeftY())
+            * Drivetrain.kMaxVelocity
+        )
+
+        # Get the rate of angular rotation. We are inverting this because we want a
+        # positive value when we pull to the left (remember, CCW is positive in
+        # mathematics). Xbox controllers return positive values when you pull to
+        # the right by default.
+        rot = (
+            -self.rotLimiter.calculate(self.controller.getRightX())
+            * Drivetrain.kMaxAngularVelocity
+        )
+
+        self.drive.drive(xVelocity, rot)

robotpyExamples/examples/DifferentialDrivePoseEstimator/drivetrain.py

@@ -0,0 +1,262 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+
+import ntcore
+import wpilib
+import wpilib.simulation
+import wpimath
+import wpimath.units
+import robotpy_apriltag
+
+
+class Drivetrain:
+    """Represents a differential drive style drivetrain."""
+
+    kMaxVelocity = 3.0  # meters per second
+    kMaxAngularVelocity = math.tau  # one rotation per second
+
+    kTrackwidth = 0.381 * 2  # meters
+    kWheelRadius = 0.0508  # meters
+    kEncoderResolution = 4096
+
+    def __init__(self, cameraToObjectTopic: ntcore.DoubleArrayTopic) -> None:
+        self.leftLeader = wpilib.PWMSparkMax(1)
+        self.leftFollower = wpilib.PWMSparkMax(2)
+        self.rightLeader = wpilib.PWMSparkMax(3)
+        self.rightFollower = wpilib.PWMSparkMax(4)
+
+        self.leftEncoder = wpilib.Encoder(0, 1)
+        self.rightEncoder = wpilib.Encoder(2, 3)
+
+        self.imu = wpilib.OnboardIMU(wpilib.OnboardIMU.MountOrientation.FLAT)
+
+        self.leftPIDController = wpimath.PIDController(1, 0, 0)
+        self.rightPIDController = wpimath.PIDController(1, 0, 0)
+
+        self.kinematics = wpimath.DifferentialDriveKinematics(self.kTrackwidth)
+
+        self.robotToCamera = wpimath.Transform3d(
+            wpimath.Translation3d(1, 1, 1),
+            wpimath.Rotation3d(0, 0, math.pi / 2),
+        )
+
+        self.defaultVal = [0.0] * 7
+        self.cameraToObjectEntry = cameraToObjectTopic.getEntry(self.defaultVal)
+
+        layout = robotpy_apriltag.AprilTagFieldLayout.loadField(
+            robotpy_apriltag.AprilTagField.k2024Crescendo
+        )
+        self.objectInField = layout.getTagPose(0) or wpimath.Pose3d()
+
+        self.fieldSim = wpilib.Field2d()
+        self.fieldApproximation = wpilib.Field2d()
+
+        # Here we use DifferentialDrivePoseEstimator so that we can fuse odometry readings. The
+        # numbers used below are robot specific, and should be tuned.
+        self.poseEstimator = wpimath.DifferentialDrivePoseEstimator(
+            self.kinematics,
+            self.imu.getRotation2d(),
+            self.leftEncoder.getDistance(),
+            self.rightEncoder.getDistance(),
+            wpimath.Pose2d(),
+            (0.05, 0.05, wpimath.units.degreesToRadians(5)),
+            (0.5, 0.5, wpimath.units.degreesToRadians(30)),
+        )
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.feedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3)
+
+        # Simulation classes
+        self.leftEncoderSim = wpilib.simulation.EncoderSim(self.leftEncoder)
+        self.rightEncoderSim = wpilib.simulation.EncoderSim(self.rightEncoder)
+        self.drivetrainSystem = wpimath.Models.differentialDriveFromSysId(
+            1.98, 0.2, 1.5, 0.3
+        )
+        self.drivetrainSimulator = wpilib.simulation.DifferentialDrivetrainSim(
+            self.drivetrainSystem,
+            self.kTrackwidth,
+            wpimath.DCMotor.CIM(2),
+            8,
+            self.kWheelRadius,
+        )
+
+        self.imu.resetYaw()
+
+        self.leftLeader.addFollower(self.leftFollower)
+        self.rightLeader.addFollower(self.rightFollower)
+
+        # We need to invert one side of the drivetrain so that positive voltages
+        # result in both sides moving forward. Depending on how your robot's
+        # gearbox is constructed, you might have to invert the left side instead.
+        self.rightLeader.setInverted(True)
+
+        # Set the distance per pulse for the drive encoders. We can simply use the
+        # distance traveled for one rotation of the wheel divided by the encoder
+        # resolution.
+        self.leftEncoder.setDistancePerPulse(
+            math.tau * self.kWheelRadius / self.kEncoderResolution
+        )
+        self.rightEncoder.setDistancePerPulse(
+            math.tau * self.kWheelRadius / self.kEncoderResolution
+        )
+
+        self.leftEncoder.reset()
+        self.rightEncoder.reset()
+
+        wpilib.SmartDashboard.putData("Field", self.fieldSim)
+        wpilib.SmartDashboard.putData("FieldEstimation", self.fieldApproximation)
+
+    def setVelocities(
+        self, velocities: wpimath.DifferentialDriveWheelVelocities
+    ) -> None:
+        """Sets the desired wheel velocities.
+
+        :param velocities: The desired wheel velocities.
+        """
+        leftFeedforward = self.feedforward.calculate(velocities.left)
+        rightFeedforward = self.feedforward.calculate(velocities.right)
+
+        leftOutput = self.leftPIDController.calculate(
+            self.leftEncoder.getRate(), velocities.left
+        )
+        rightOutput = self.rightPIDController.calculate(
+            self.rightEncoder.getRate(), velocities.right
+        )
+        self.leftLeader.setVoltage(leftOutput + leftFeedforward)
+        self.rightLeader.setVoltage(rightOutput + rightFeedforward)
+
+    def drive(self, xVelocity: float, rot: float) -> None:
+        """Drives the robot with the given linear velocity and angular velocity.
+
+        :param xVelocity: Linear velocity in m/s.
+        :param rot: Angular velocity in rad/s.
+        """
+        wheelVelocities = self.kinematics.toWheelVelocities(
+            wpimath.ChassisVelocities(xVelocity, 0.0, rot)
+        )
+        self.setVelocities(wheelVelocities)
+
+    def publishCameraToObject(
+        self,
+        objectInField: wpimath.Pose3d,
+        robotToCamera: wpimath.Transform3d,
+        cameraToObjectEntry: ntcore.DoubleArrayEntry,
+        drivetrainSimulator: wpilib.simulation.DifferentialDrivetrainSim,
+    ) -> None:
+        """Computes and publishes to a network tables topic the transformation from the
+        camera's pose to the object's pose. This function exists solely for the
+        purposes of simulation, and this would normally be handled by computer vision.
+
+        The object could be a target or a fiducial marker.
+
+        :param objectInField: The object's field-relative position.
+        :param robotToCamera: The transformation from the robot's pose to the camera's pose.
+        :param cameraToObjectEntry: The networktables entry publishing and querying example
+            computer vision measurements.
+        """
+        robotInField = wpimath.Pose3d(drivetrainSimulator.getPose())
+        cameraInField = robotInField.transformBy(robotToCamera)
+        cameraToObject = wpimath.Transform3d(cameraInField, objectInField)
+
+        # Publishes double array with Translation3D elements {x, y, z} and Rotation3D elements
+        # {w, x, y, z} which describe the cameraToObject transformation.
+        quaternion = cameraToObject.rotation().getQuaternion()
+        val = [
+            cameraToObject.x,
+            cameraToObject.y,
+            cameraToObject.z,
+            quaternion.w,
+            quaternion.x,
+            quaternion.y,
+            quaternion.z,
+        ]
+        cameraToObjectEntry.set(val)
+
+    def objectToRobotPose(
+        self,
+        objectInField: wpimath.Pose3d,
+        robotToCamera: wpimath.Transform3d,
+        cameraToObjectEntry: ntcore.DoubleArrayEntry,
+    ) -> wpimath.Pose3d:
+        """Queries the camera-to-object transformation from networktables to compute the robot's
+        field-relative pose from vision measurements.
+
+        The object could be a target or a fiducial marker.
+
+        :param objectInField: The object's field-relative pose.
+        :param robotToCamera: The transformation from the robot's pose to the camera's pose.
+        :param cameraToObjectEntry: The networktables entry publishing and querying example
+            computer vision measurements.
+        """
+        val = cameraToObjectEntry.get()
+
+        # Reconstruct cameraToObject Transform3d from networktables.
+        translation = wpimath.Translation3d(val[0], val[1], val[2])
+        rotation = wpimath.Rotation3d(
+            wpimath.Quaternion(val[3], val[4], val[5], val[6])
+        )
+        cameraToObject = wpimath.Transform3d(translation, rotation)
+
+        cameraInField = objectInField.transformBy(cameraToObject.inverse())
+        robotInField = cameraInField.transformBy(robotToCamera.inverse())
+        return robotInField
+
+    def updateOdometry(self) -> None:
+        """Updates the field-relative position."""
+        self.poseEstimator.update(
+            self.imu.getRotation2d(),
+            self.leftEncoder.getDistance(),
+            self.rightEncoder.getDistance(),
+        )
+
+        # Publish cameraToObject transformation to networktables --this would normally be handled by
+        # the computer vision solution.
+        self.publishCameraToObject(
+            self.objectInField,
+            self.robotToCamera,
+            self.cameraToObjectEntry,
+            self.drivetrainSimulator,
+        )
+
+        # Compute the robot's field-relative position exclusively from vision measurements.
+        visionMeasurement3d = self.objectToRobotPose(
+            self.objectInField, self.robotToCamera, self.cameraToObjectEntry
+        )
+
+        # Convert robot pose from Pose3d to Pose2d needed to apply vision measurements.
+        visionMeasurement2d = visionMeasurement3d.toPose2d()
+
+        # Apply vision measurements. For simulation purposes only, we don't input a latency delay --
+        # on a real robot, this must be calculated based either on known latency or timestamps.
+        self.poseEstimator.addVisionMeasurement(
+            visionMeasurement2d,
+            wpilib.Timer.getTimestamp(),
+        )
+
+    def simulationPeriodic(self) -> None:
+        """This function is called periodically during simulation."""
+        # To update our simulation, we set motor voltage inputs, update the
+        # simulation, and write the simulated positions and velocities to our
+        # simulated encoder and gyro.
+        self.drivetrainSimulator.setInputs(
+            self.leftLeader.getThrottle() * wpilib.RobotController.getInputVoltage(),
+            self.rightLeader.getThrottle() * wpilib.RobotController.getInputVoltage(),
+        )
+        self.drivetrainSimulator.update(0.02)
+
+        self.leftEncoderSim.setDistance(self.drivetrainSimulator.getLeftPosition())
+        self.leftEncoderSim.setRate(self.drivetrainSimulator.getLeftVelocity())
+        self.rightEncoderSim.setDistance(self.drivetrainSimulator.getRightPosition())
+        self.rightEncoderSim.setRate(self.drivetrainSimulator.getRightVelocity())
+        # self.gyroSim.setAngle(-self.drivetrainSimulator.getHeading().getDegrees())
+
+    def periodic(self) -> None:
+        """This function is called periodically, no matter the mode."""
+        self.updateOdometry()
+        self.fieldSim.setRobotPose(self.drivetrainSimulator.getPose())
+        self.fieldApproximation.setRobotPose(self.poseEstimator.getEstimatedPosition())

robotpyExamples/examples/DifferentialDrivePoseEstimator/exampleglobalmeasurementsensor.py

@@ -0,0 +1,34 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import random
+
+import wpimath
+import wpimath.units
+
+
+class ExampleGlobalMeasurementSensor:
+    """This dummy class represents a global measurement sensor, such as a computer vision
+    solution.
+    """
+
+    def __init__(self) -> None:
+        raise RuntimeError("Utility class")
+
+    @staticmethod
+    def getEstimatedGlobalPose(estimatedRobotPose: wpimath.Pose2d) -> wpimath.Pose2d:
+        """Get a "noisy" fake global pose reading.
+
+        :param estimatedRobotPose: The robot pose.
+        """
+        rand_x = random.gauss(0.0, 0.5)
+        rand_y = random.gauss(0.0, 0.5)
+        rand_rot = random.gauss(0.0, wpimath.units.degreesToRadians(30))
+        return wpimath.Pose2d(
+            estimatedRobotPose.x + rand_x,
+            estimatedRobotPose.y + rand_y,
+            estimatedRobotPose.rotation().rotateBy(wpimath.Rotation2d(rand_rot)),
+        )

robotpyExamples/examples/DifferentialDrivePoseEstimator/robot.py

@@ -0,0 +1,52 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import ntcore
+import wpilib
+import wpimath
+
+from drivetrain import Drivetrain
+
+
+class MyRobot(wpilib.TimedRobot):
+    def __init__(self) -> None:
+        super().__init__()
+
+        self.inst = ntcore.NetworkTableInstance.getDefault()
+        self.doubleArrayTopic = self.inst.getDoubleArrayTopic("doubleArrayTopic")
+
+        self.controller = wpilib.NiDsXboxController(0)
+        self.drive = Drivetrain(self.doubleArrayTopic)
+
+        # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+        self.velocityLimiter = wpimath.SlewRateLimiter(3)
+        self.rotLimiter = wpimath.SlewRateLimiter(3)
+
+    def autonomousPeriodic(self) -> None:
+        self.teleopPeriodic()
+        self.drive.updateOdometry()
+
+    def simulationPeriodic(self) -> None:
+        self.drive.simulationPeriodic()
+
+    def robotPeriodic(self) -> None:
+        self.drive.periodic()
+
+    def teleopPeriodic(self) -> None:
+        # Get the x velocity. We are inverting this because Xbox controllers return
+        # negative values when we push forward.
+        xVelocity = -self.velocityLimiter.calculate(self.controller.getLeftY())
+        xVelocity *= Drivetrain.kMaxVelocity
+
+        # Get the rate of angular rotation. We are inverting this because we want a
+        # positive value when we pull to the left (remember, CCW is positive in
+        # mathematics). Xbox controllers return positive values when you pull to
+        # the right by default.
+        rot = -self.rotLimiter.calculate(self.controller.getRightX())
+        rot *= Drivetrain.kMaxAngularVelocity
+
+        self.drive.drive(xVelocity, rot)

robotpyExamples/examples/DriveDistanceOffboard/constants.py

@@ -0,0 +1,40 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+"""
+A place for the constant values in the code that may be used in more than one place.
+This offers a convenient resources to teams who need to make both quick and universal
+changes.
+"""
+
+import math
+
+
+class DriveConstants:
+    kDt = 0.02
+    kLeftMotor1Port = 0
+    kLeftMotor2Port = 1
+    kRightMotor1Port = 2
+    kRightMotor2Port = 3
+
+    """
+    These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+    These characterization values MUST be determined either experimentally or theoretically
+    for *your* robot's drive.
+    The SysId tool provides a convenient method for obtaining these values for your robot.
+    """
+    ks = 1.0  # V
+    kv = 0.8  # V/(m/s)
+    ka = 0.15  # V/(m/s²)
+
+    kp = 1.0
+
+    kMaxVelocity = 3.0  # m/s
+    kMaxAcceleration = 3.0  # m/s²
+
+
+class OIConstants:
+    kDriverControllerPort = 0

robotpyExamples/examples/DriveDistanceOffboard/examplesmartmotorcontroller.py

@@ -0,0 +1,98 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import enum
+
+
+class ExampleSmartMotorController:
+    """A simplified stub class that simulates the API of a common "smart" motor controller.
+    Has no actual functionality.
+    """
+
+    class PIDMode(enum.Enum):
+        kPosition = enum.auto()
+        kVelocity = enum.auto()
+        kMovementWitchcraft = enum.auto()
+
+    def __init__(self, port: int) -> None:
+        """Creates a new ExampleSmartMotorController.
+
+        Args:
+            port: The port for the controller.
+        """
+        super().__init__()
+        self._velocity = 0.0
+        self._inverted = False
+        self._leader = None
+
+    def setPID(self, kp: float, ki: float, kd: float) -> None:
+        """Example method for setting the PID gains of the smart controller.
+
+        Args:
+            kp: The proportional gain.
+            ki: The integral gain.
+            kd: The derivative gain.
+        """
+        pass
+
+    def setSetPoint(
+        self, mode: PIDMode, setpoint: float, arbfeedforward: float
+    ) -> None:
+        """Example method for setting the setpoint of the smart controller in PID mode.
+
+        Args:
+            mode: The mode of the PID controller.
+            setpoint: The controller setpoint.
+            arbfeedforward: An arbitrary feedforward output (from -1 to 1).
+        """
+        pass
+
+    def follow(self, leader: "ExampleSmartMotorController") -> None:
+        """Places this motor controller in follower mode.
+
+        Args:
+            leader: The leader to follow.
+        """
+        self._leader = leader
+
+    def getEncoderDistance(self) -> float:
+        """Returns the encoder distance.
+
+        Returns:
+            The current encoder distance.
+        """
+        return 0
+
+    def getEncoderRate(self) -> float:
+        """Returns the encoder rate.
+
+        Returns:
+            The current encoder rate.
+        """
+        return 0
+
+    def resetEncoder(self) -> None:
+        """Resets the encoder to zero distance."""
+        pass
+
+    def set(self, velocity: float) -> None:
+        self._velocity = -velocity if self._inverted else velocity
+
+    def get(self) -> float:
+        return self._velocity
+
+    def setInverted(self, isInverted: bool) -> None:
+        self._inverted = isInverted
+
+    def getInverted(self) -> bool:
+        return self._inverted
+
+    def disable(self) -> None:
+        self._velocity = 0.0
+
+    def stopMotor(self) -> None:
+        self._velocity = 0.0

robotpyExamples/examples/DriveDistanceOffboard/robot.py

@@ -0,0 +1,74 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import typing
+
+import wpilib
+import commands2
+import commands2.cmd
+
+import robotcontainer
+
+"""
+The VM is configured to automatically run this class, and to call the functions corresponding to
+each mode, as described in the TimedRobot documentation. If you change the name of this class or
+the package after creating this project, you must also update the build.gradle file in the
+project.
+"""
+
+
+class MyRobot(commands2.TimedCommandRobot):
+    """
+    Command v2 robots are encouraged to inherit from TimedCommandRobot, which
+    has an implementation of robotPeriodic which runs the scheduler for you
+    """
+
+    def __init__(self) -> None:
+        """
+        This function is run when the robot is first started up and should be used for any
+        initialization code.
+        """
+        super().__init__()
+        self.autonomousCommand: typing.Optional[commands2.Command] = None
+
+        # Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+        # autonomous chooser on the dashboard.
+        self.container = robotcontainer.RobotContainer()
+
+    def disabledInit(self) -> None:
+        """This function is called once each time the robot enters Disabled mode."""
+
+    def disabledPeriodic(self) -> None:
+        """This function is called periodically when disabled"""
+
+    def autonomousInit(self) -> None:
+        """This autonomous runs the autonomous command selected by your RobotContainer class."""
+        self.autonomousCommand = self.container.getAutonomousCommand()
+
+        # schedule the autonomous command (example)
+        if self.autonomousCommand is not None:
+            self.autonomousCommand.schedule()
+        else:
+            print("no auto command?")
+
+    def autonomousPeriodic(self) -> None:
+        """This function is called periodically during autonomous"""
+
+    def teleopInit(self) -> None:
+        # This makes sure that the autonomous stops running when
+        # teleop starts running. If you want the autonomous to
+        # continue until interrupted by another command, remove
+        # this line or comment it out.
+        if self.autonomousCommand is not None:
+            self.autonomousCommand.cancel()
+
+    def teleopPeriodic(self) -> None:
+        """This function is called periodically during operator control"""
+
+    def utilityInit(self) -> None:
+        # Cancels all running commands at the start of utility mode
+        commands2.CommandScheduler.getInstance().cancelAll()

robotpyExamples/examples/DriveDistanceOffboard/robotcontainer.py

@@ -0,0 +1,89 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+import commands2.cmd
+import commands2.button
+import constants
+import subsystems.drivesubsystem
+
+
+class RobotContainer:
+    """
+    This class is where the bulk of the robot should be declared. Since Command-based is a
+    "declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot`
+    periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+    subsystems, commands, and button mappings) should be declared here.
+
+    """
+
+    def __init__(self):
+        # The robot's subsystems
+        self.robotDrive = subsystems.drivesubsystem.DriveSubsystem()
+
+        # Retained command references
+        self.driveFullVelocity = commands2.cmd.runOnce(
+            lambda: self.robotDrive.setMaxOutput(1), self.robotDrive
+        )
+        self.driveHalfVelocity = commands2.cmd.runOnce(
+            lambda: self.robotDrive.setMaxOutput(0.5), self.robotDrive
+        )
+
+        # The driver's controller
+        self.driverController = commands2.button.CommandNiDsXboxController(
+            constants.OIConstants.kDriverControllerPort
+        )
+
+        # Configure the button bindings
+        self.configureButtonBindings()
+
+        # Configure default commands
+        # Set the default drive command to split-stick arcade drive
+        self.robotDrive.setDefaultCommand(
+            # A split-stick arcade command, with forward/backward controlled by the left
+            # hand, and turning controlled by the right.
+            commands2.cmd.run(
+                lambda: self.robotDrive.arcadeDrive(
+                    -self.driverController.getLeftY(),
+                    -self.driverController.getRightX(),
+                ),
+                self.robotDrive,
+            )
+        )
+
+    def configureButtonBindings(self):
+        """
+        Use this method to define your button->command mappings. Buttons can be created via the button
+        factories on commands2.button.CommandGenericHID or one of its
+        subclasses (commands2.button.CommandJoystick or command2.button.CommandNiDsXboxController).
+        """
+
+        # Configure your button bindings here
+
+        # We can bind commands while retaining references to them in RobotContainer
+
+        # Drive at half velocity when the bumper is held
+        self.driverController.rightBumper().onTrue(self.driveHalfVelocity).onFalse(
+            self.driveFullVelocity
+        )
+
+        # Drive forward by 3 meters when the 'A' button is pressed, with a timeout of 10 seconds
+        self.driverController.a().onTrue(
+            self.robotDrive.profiledDriveDistance(3).withTimeout(10)
+        )
+
+        # Do the same thing as above when the 'B' button is pressed, but defined inline
+        self.driverController.b().onTrue(
+            self.robotDrive.dynamicProfiledDriveDistance(3).withTimeout(10)
+        )
+
+    def getAutonomousCommand(self) -> commands2.Command:
+        """
+        Use this to pass the autonomous command to the main :class:`.Robot` class.
+
+        :returns: the command to run in autonomous
+        """
+        return commands2.cmd.none()

robotpyExamples/examples/DriveDistanceOffboard/subsystems/drivesubsystem.py

@@ -0,0 +1,223 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import commands2
+import constants
+import examplesmartmotorcontroller
+import wpimath
+
+
+class DriveSubsystem(commands2.Subsystem):
+    def __init__(self) -> None:
+        """Creates a new DriveSubsystem"""
+        super().__init__()
+
+        # The motors on the left side of the drive.
+        self.leftLeader = examplesmartmotorcontroller.ExampleSmartMotorController(
+            constants.DriveConstants.kLeftMotor1Port
+        )
+
+        self.leftFollower = examplesmartmotorcontroller.ExampleSmartMotorController(
+            constants.DriveConstants.kLeftMotor2Port
+        )
+
+        # The motors on the right side of the drive.
+        self.rightLeader = examplesmartmotorcontroller.ExampleSmartMotorController(
+            constants.DriveConstants.kRightMotor1Port
+        )
+
+        self.rightFollower = examplesmartmotorcontroller.ExampleSmartMotorController(
+            constants.DriveConstants.kRightMotor1Port
+        )
+
+        # We need to invert one side of the drivetrain so that positive voltages
+        # result in both sides moving forward. Depending on how your robot's
+        # gearbox is constructed, you might have to invert the left side instead.
+        self.rightLeader.setInverted(True)
+
+        # You might need to not do this depending on the specific motor controller
+        # that you are using -- contact the respective vendor's documentation for
+        # more details.
+        self.rightFollower.setInverted(True)
+
+        self.leftFollower.follow(self.leftLeader)
+        self.rightFollower.follow(self.rightLeader)
+
+        self.leftLeader.setPID(constants.DriveConstants.kp, 0, 0)
+        self.rightLeader.setPID(constants.DriveConstants.kp, 0, 0)
+
+        # The feedforward controller (note that these are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!)
+        # check DriveConstants for more information.
+        self.feedforward = wpimath.SimpleMotorFeedforwardMeters(
+            constants.DriveConstants.ks,
+            constants.DriveConstants.kv,
+            constants.DriveConstants.ka,
+        )
+
+        # The robot's drive
+        self.drive = wpilib.DifferentialDrive(self.leftLeader.set, self.rightLeader.set)
+
+        self.profile = wpimath.TrapezoidProfile(
+            wpimath.TrapezoidProfile.Constraints(
+                constants.DriveConstants.kMaxVelocity,
+                constants.DriveConstants.kMaxAcceleration,
+            )
+        )
+        self.timer = wpilib.Timer()
+        self._initialLeftDistance = 0.0
+        self._initialRightDistance = 0.0
+
+    def arcadeDrive(self, fwd: float, rot: float):
+        """
+        Drives the robot using arcade controls.
+
+        :param fwd: the commanded forward movement
+        :param rot: the commanded rotation
+        """
+        self.drive.arcadeDrive(fwd, rot)
+
+    def setDriveStates(
+        self,
+        currentLeft: wpimath.TrapezoidProfile.State,
+        currentRight: wpimath.TrapezoidProfile.State,
+        nextLeft: wpimath.TrapezoidProfile.State,
+        nextRight: wpimath.TrapezoidProfile.State,
+    ):
+        """
+        Attempts to follow the given drive states using offboard PID.
+
+        :param currentLeft: The current left wheel state.
+        :param currentRight: The current right wheel state.
+        :param nextLeft: The next left wheel state.
+        :param nextRight: The next right wheel state.
+        """
+        battery_voltage = wpilib.RobotController.getBatteryVoltage()
+        left_feedforward = self.feedforward.calculate(
+            currentLeft.velocity,
+            (nextLeft.velocity - currentLeft.velocity) / constants.DriveConstants.kDt,
+        )
+        right_feedforward = self.feedforward.calculate(
+            currentRight.velocity,
+            (nextRight.velocity - currentRight.velocity) / constants.DriveConstants.kDt,
+        )
+        self.leftLeader.setSetPoint(
+            examplesmartmotorcontroller.ExampleSmartMotorController.PIDMode.kPosition,
+            currentLeft.position,
+            left_feedforward / battery_voltage,
+        )
+
+        self.rightLeader.setSetPoint(
+            examplesmartmotorcontroller.ExampleSmartMotorController.PIDMode.kPosition,
+            currentRight.position,
+            right_feedforward / battery_voltage,
+        )
+
+    def getLeftEncoderDistance(self) -> float:
+        """
+        Returns the left drive encoder distance.
+
+        :returns: the left drive encoder distance
+        """
+        return self.leftLeader.getEncoderDistance()
+
+    def getRightEncoderDistance(self) -> float:
+        """
+        Returns the right drive encoder distance.
+
+        :returns: the right drive encoder distance
+        """
+        return self.rightLeader.getEncoderDistance()
+
+    def resetEncoders(self):
+        """Resets the drive encoders"""
+        self.leftLeader.resetEncoder()
+        self.rightLeader.resetEncoder()
+
+    def setMaxOutput(self, maxOutput: float):
+        """
+        Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+
+        :param maxOutput: the maximum output to which the drive will be constrained
+        """
+        self.drive.setMaxOutput(maxOutput)
+
+    def profiledDriveDistance(self, distance: float) -> commands2.Command:
+        def on_init():
+            self.timer.restart()
+            self.resetEncoders()
+
+        def on_execute():
+            current_time = self.timer.get()
+            current_setpoint = self.profile.calculate(
+                current_time,
+                wpimath.TrapezoidProfile.State(),
+                wpimath.TrapezoidProfile.State(distance, 0),
+            )
+            next_setpoint = self.profile.calculate(
+                current_time + constants.DriveConstants.kDt,
+                wpimath.TrapezoidProfile.State(),
+                wpimath.TrapezoidProfile.State(distance, 0),
+            )
+            self.setDriveStates(
+                current_setpoint, current_setpoint, next_setpoint, next_setpoint
+            )
+
+        def on_end(interrupted: bool):
+            self.leftLeader.set(0)
+            self.rightLeader.set(0)
+
+        def is_finished() -> bool:
+            return self.profile.isFinished(0)
+
+        return commands2.FunctionalCommand(
+            on_init, on_execute, on_end, is_finished, self
+        )
+
+    def dynamicProfiledDriveDistance(self, distance: float) -> commands2.Command:
+        def on_init():
+            self.timer.restart()
+            self._initialLeftDistance = self.getLeftEncoderDistance()
+            self._initialRightDistance = self.getRightEncoderDistance()
+
+        def on_execute():
+            current_time = self.timer.get()
+            current_left = self.profile.calculate(
+                current_time,
+                wpimath.TrapezoidProfile.State(self._initialLeftDistance, 0),
+                wpimath.TrapezoidProfile.State(self._initialLeftDistance + distance, 0),
+            )
+            current_right = self.profile.calculate(
+                current_time,
+                wpimath.TrapezoidProfile.State(self._initialRightDistance, 0),
+                wpimath.TrapezoidProfile.State(
+                    self._initialRightDistance + distance, 0
+                ),
+            )
+            next_left = self.profile.calculate(
+                current_time + constants.DriveConstants.kDt,
+                wpimath.TrapezoidProfile.State(self._initialLeftDistance, 0),
+                wpimath.TrapezoidProfile.State(self._initialLeftDistance + distance, 0),
+            )
+            next_right = self.profile.calculate(
+                current_time + constants.DriveConstants.kDt,
+                wpimath.TrapezoidProfile.State(self._initialRightDistance, 0),
+                wpimath.TrapezoidProfile.State(
+                    self._initialRightDistance + distance, 0
+                ),
+            )
+            self.setDriveStates(current_left, current_right, next_left, next_right)
+
+        def on_end(interrupted: bool):
+            self.leftLeader.set(0)
+            self.rightLeader.set(0)
+
+        def is_finished() -> bool:
+            return self.profile.isFinished(0)
+
+        return commands2.FunctionalCommand(
+            on_init, on_execute, on_end, is_finished, self
+        )

robotpyExamples/examples/DutyCycleEncoder/robot.py

@@ -0,0 +1,74 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+"""
+This example shows how to use a duty cycle encoder for devices such as
+an arm or elevator.
+"""
+
+import wpilib
+import wpimath
+
+FULL_RANGE = 1.3
+EXPECTED_ZERO = 0.0
+
+
+class MyRobot(wpilib.TimedRobot):
+    def __init__(self):
+        """Called once at the beginning of the robot program."""
+        super().__init__()
+
+        # 2nd parameter is the range of values. This sensor will output between
+        # 0 and the passed in value.
+        # 3rd parameter is the the physical value where you want "0" to be. How
+        # to measure this is fairly easy. Set the value to 0, place the mechanism
+        # where you want "0" to be, and observe the value on the dashboard, That
+        # is the value to enter for the 3rd parameter.
+        self.dutyCycleEncoder = wpilib.DutyCycleEncoder(0, FULL_RANGE, EXPECTED_ZERO)
+
+        # If you know the frequency of your sensor, uncomment the following
+        # method, and set the method to the frequency of your sensor.
+        # This will result in more stable readings from the sensor.
+        # Do note that occasionally the datasheet cannot be trusted
+        # and you should measure this value. You can do so with either
+        # an oscilloscope, or by observing the "Frequency" output
+        # on the dashboard while running this sample. If you find
+        # the value jumping between the 2 values, enter halfway between
+        # those values. This number doesn't have to be perfect,
+        # just having a fairly close value will make the output readings
+        # much more stable.
+        self.dutyCycleEncoder.setAssumedFrequency(967.8)
+
+    def robotPeriodic(self):
+        # Connected can be checked, and uses the frequency of the encoder
+        connected = self.dutyCycleEncoder.isConnected()
+
+        # Duty Cycle Frequency in Hz
+        frequency = self.dutyCycleEncoder.getFrequency()
+
+        # Output of encoder
+        output = self.dutyCycleEncoder.get()
+
+        # By default, the output will wrap around to the full range value
+        # when the sensor goes below 0. However, for moving mechanisms this
+        # is not usually ideal, as if 0 is set to a hard stop, its still
+        # possible for the sensor to move slightly past. If this happens
+        # The sensor will assume its now at the furthest away position,
+        # which control algorithms might not handle correctly. Therefore
+        # it can be a good idea to slightly shift the output so the sensor
+        # can go a bit negative before wrapping. Usually 10% or so is fine.
+        # This does not change where "0" is, so no calibration numbers need
+        # to be changed.
+        percentOfRange = FULL_RANGE * 0.1
+        shiftedOutput = wpimath.inputModulus(
+            output, 0 - percentOfRange, FULL_RANGE - percentOfRange
+        )
+
+        wpilib.SmartDashboard.putBoolean("Connected", connected)
+        wpilib.SmartDashboard.putNumber("Frequency", frequency)
+        wpilib.SmartDashboard.putNumber("Output", output)
+        wpilib.SmartDashboard.putNumber("ShiftedOutput", shiftedOutput)

robotpyExamples/examples/ElevatorExponentialProfile/examplesmartmotorcontroller.py

@@ -0,0 +1,93 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import enum
+
+
+class ExampleSmartMotorController:
+    """A simplified stub class that simulates the API of a common "smart" motor controller.
+
+    Has no actual functionality.
+    """
+
+    class PIDMode(enum.Enum):
+        kPosition = enum.auto()
+        kVelocity = enum.auto()
+        kMovementWitchcraft = enum.auto()
+
+    def __init__(self, port: int) -> None:
+        """Creates a new ExampleSmartMotorController.
+
+        :param port: The port for the controller.
+        """
+        self._port = port
+        self._inverted = False
+
+    def setPID(self, kp: float, ki: float, kd: float) -> None:
+        """Example method for setting the PID gains of the smart controller.
+
+        :param kp: The proportional gain.
+        :param ki: The integral gain.
+        :param kd: The derivative gain.
+        """
+        pass
+
+    def setSetpoint(
+        self,
+        mode: "ExampleSmartMotorController.PIDMode",
+        setpoint: float,
+        arbFeedforward: float,
+    ) -> None:
+        """Example method for setting the setpoint of the smart controller in PID mode.
+
+        :param mode: The mode of the PID controller.
+        :param setpoint: The controller setpoint.
+        :param arbFeedforward: An arbitrary feedforward output (from -1 to 1).
+        """
+        pass
+
+    def follow(self, leader: "ExampleSmartMotorController") -> None:
+        """Places this motor controller in follower mode.
+
+        :param leader: The leader to follow.
+        """
+        pass
+
+    def getEncoderDistance(self) -> float:
+        """Returns the encoder distance.
+
+        :returns: The current encoder distance.
+        """
+        return 0
+
+    def getEncoderRate(self) -> float:
+        """Returns the encoder rate.
+
+        :returns: The current encoder rate.
+        """
+        return 0
+
+    def resetEncoder(self) -> None:
+        """Resets the encoder to zero distance."""
+        pass
+
+    def set(self, velocity: float) -> None:
+        pass
+
+    def get(self) -> float:
+        return 0
+
+    def setInverted(self, isInverted: bool) -> None:
+        self._inverted = isInverted
+
+    def getInverted(self) -> bool:
+        return self._inverted
+
+    def disable(self) -> None:
+        pass
+
+    def stopMotor(self) -> None:
+        pass

robotpyExamples/examples/ElevatorExponentialProfile/robot.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import wpimath
+
+from examplesmartmotorcontroller import ExampleSmartMotorController
+
+
+class MyRobot(wpilib.TimedRobot):
+    kDt = 0.02
+
+    def __init__(self) -> None:
+        super().__init__()
+        self.joystick = wpilib.Joystick(1)
+        self.motor = ExampleSmartMotorController(1)
+        # Note: These gains are fake, and will have to be tuned for your robot.
+        self.feedforward = wpimath.SimpleMotorFeedforwardMeters(1, 1, 1)
+
+        # Create a motion profile with the given maximum voltage and characteristics kV, kA
+        # These gains should match your feedforward kV, kA for best results.
+        self.profile = wpimath.ExponentialProfileMeterVolts(
+            wpimath.ExponentialProfileMeterVolts.Constraints.fromCharacteristics(
+                10, 1, 1
+            )
+        )
+        self.goal = wpimath.ExponentialProfileMeterVolts.State(0, 0)
+        self.setpoint = wpimath.ExponentialProfileMeterVolts.State(0, 0)
+
+        # Note: These gains are fake, and will have to be tuned for your robot.
+        self.motor.setPID(1.3, 0.0, 0.7)
+
+    def teleopPeriodic(self) -> None:
+        if self.joystick.getRawButtonPressed(2):
+            self.goal = wpimath.ExponentialProfileMeterVolts.State(5, 0)
+        elif self.joystick.getRawButtonPressed(3):
+            self.goal = wpimath.ExponentialProfileMeterVolts.State(0, 0)
+
+        # Retrieve the profiled setpoint for the next timestep. This setpoint moves
+        # toward the goal while obeying the constraints.
+        next_state = self.profile.calculate(self.kDt, self.setpoint, self.goal)
+
+        # Send setpoint to offboard controller PID
+        self.motor.setSetpoint(
+            ExampleSmartMotorController.PIDMode.kPosition,
+            self.setpoint.position,
+            self.feedforward.calculate(next_state.velocity) / 12.0,
+        )
+
+        self.setpoint = next_state

robotpyExamples/examples/ElevatorExponentialSimulation/constants.py

@@ -0,0 +1,38 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+
+import wpimath.units
+
+kMotorPort = 0
+kEncoderAChannel = 0
+kEncoderBChannel = 1
+kJoystickPort = 0
+
+kElevatorKp = 0.75
+kElevatorKi = 0.0
+kElevatorKd = 0.0
+
+kElevatorMaxV = 10.0  # volts (V)
+kElevatorkS = 0.0  # volts (V)
+kElevatorkG = 0.62  # volts (V)
+kElevatorkV = 3.9  # volts (V)
+kElevatorkA = 0.06  # volts (V)
+
+kElevatorGearing = 5.0
+kElevatorDrumRadius = wpimath.units.inchesToMeters(1.0)
+kCarriageMass = wpimath.units.lbsToKilograms(12)  # kg
+
+kSetpoint = wpimath.units.inchesToMeters(42.875)
+kLowerkSetpoint = wpimath.units.inchesToMeters(15)
+# Encoder is reset to measure 0 at the bottom, so minimum height is 0.
+kMinElevatorHeight = 0.0  # m
+kMaxElevatorHeight = wpimath.units.inchesToMeters(50)
+
+# distance per pulse = (distance per revolution) / (pulses per revolution)
+#  = (Pi * D) / ppr
+kElevatorEncoderDistPerPulse = 2.0 * math.pi * kElevatorDrumRadius / 4096

robotpyExamples/examples/ElevatorExponentialSimulation/robot.py

@@ -0,0 +1,43 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+
+import constants
+from subsystems.elevator import Elevator
+
+
+class MyRobot(wpilib.TimedRobot):
+    """This is a sample program to demonstrate the use of elevator simulation."""
+
+    def __init__(self) -> None:
+        super().__init__(0.020)
+        self.joystick = wpilib.Joystick(constants.kJoystickPort)
+        self.elevator = Elevator()
+
+    def robotPeriodic(self) -> None:
+        # Update the telemetry, including mechanism visualization, regardless of mode.
+        self.elevator.updateTelemetry()
+
+    def simulationPeriodic(self) -> None:
+        # Update the simulation model.
+        self.elevator.simulationPeriodic()
+
+    def teleopInit(self) -> None:
+        self.elevator.reset()
+
+    def teleopPeriodic(self) -> None:
+        if self.joystick.getTrigger():
+            # Here, we set the constant setpoint of 10 meters.
+            self.elevator.reachGoal(constants.kSetpoint)
+        else:
+            # Otherwise, we update the setpoint to 1 meter.
+            self.elevator.reachGoal(constants.kLowerkSetpoint)
+
+    def disabledInit(self) -> None:
+        # This just makes sure that our simulation code knows that the motor's off.
+        self.elevator.stop()

robotpyExamples/examples/ElevatorExponentialSimulation/subsystems/elevator.py

@@ -0,0 +1,134 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import wpilib.simulation
+import wpimath
+import wpimath.units
+
+import constants
+
+
+class Elevator:
+    def __init__(self) -> None:
+        # This gearbox represents a gearbox containing 4 Vex 775pro motors.
+        self.elevatorGearbox = wpimath.DCMotor.NEO(2)
+
+        self.profile = wpimath.ExponentialProfileMeterVolts(
+            wpimath.ExponentialProfileMeterVolts.Constraints.fromCharacteristics(
+                constants.kElevatorMaxV,
+                constants.kElevatorkV,
+                constants.kElevatorkA,
+            )
+        )
+
+        self.setpoint = wpimath.ExponentialProfileMeterVolts.State(0, 0)
+
+        # Standard classes for controlling our elevator
+        self.pidController = wpimath.PIDController(
+            constants.kElevatorKp, constants.kElevatorKi, constants.kElevatorKd
+        )
+
+        self.feedforward = wpimath.ElevatorFeedforward(
+            constants.kElevatorkS,
+            constants.kElevatorkG,
+            constants.kElevatorkV,
+            constants.kElevatorkA,
+        )
+        self.encoder = wpilib.Encoder(
+            constants.kEncoderAChannel, constants.kEncoderBChannel
+        )
+        self.motor = wpilib.PWMSparkMax(constants.kMotorPort)
+
+        # Simulation classes help us simulate what's going on, including gravity.
+        self.elevatorSim = wpilib.simulation.ElevatorSim(
+            self.elevatorGearbox,
+            constants.kElevatorGearing,
+            constants.kCarriageMass,
+            constants.kElevatorDrumRadius,
+            constants.kMinElevatorHeight,
+            constants.kMaxElevatorHeight,
+            True,
+            0,
+            [0.005, 0.0],
+        )
+        self.encoderSim = wpilib.simulation.EncoderSim(self.encoder)
+        self.motorSim = wpilib.simulation.PWMMotorControllerSim(self.motor)
+
+        # Create a Mechanism2d visualization of the elevator
+        self.mech2d = wpilib.Mechanism2d(
+            wpimath.units.inchesToMeters(10),
+            wpimath.units.inchesToMeters(51),
+        )
+        self.mech2dRoot = self.mech2d.getRoot(
+            "Elevator Root",
+            wpimath.units.inchesToMeters(5),
+            wpimath.units.inchesToMeters(0.5),
+        )
+        self.elevatorMech2d = self.mech2dRoot.appendLigament(
+            "Elevator", self.elevatorSim.getPosition(), 90
+        )
+
+        # Subsystem constructor.
+        self.encoder.setDistancePerPulse(constants.kElevatorEncoderDistPerPulse)
+
+        # Publish Mechanism2d to SmartDashboard
+        # To view the Elevator visualization, select Network Tables -> SmartDashboard -> Elevator Sim
+        wpilib.SmartDashboard.putData("Elevator Sim", self.mech2d)
+
+    def simulationPeriodic(self) -> None:
+        """Advance the simulation."""
+        # In this method, we update our simulation of what our elevator is doing
+        # First, we set our "inputs" (voltages)
+        self.elevatorSim.setInputVoltage(
+            self.motorSim.getThrottle() * wpilib.RobotController.getBatteryVoltage()
+        )
+
+        # Next, we update it. The standard loop time is 20ms.
+        self.elevatorSim.update(0.020)
+
+        # Finally, we set our simulated encoder's readings and simulated battery voltage
+        self.encoderSim.setDistance(self.elevatorSim.getPosition())
+        # SimBattery estimates loaded battery voltages
+        wpilib.simulation.RoboRioSim.setVInVoltage(
+            wpilib.simulation.BatterySim.calculate([self.elevatorSim.getCurrentDraw()])
+        )
+
+    def reachGoal(self, goal: float) -> None:
+        """Run control loop to reach and maintain goal.
+
+        :param goal: the position to maintain
+        """
+        goalState = wpimath.ExponentialProfileMeterVolts.State(goal, 0)
+
+        next_state = self.profile.calculate(0.020, self.setpoint, goalState)
+
+        # With the setpoint value we run PID control like normal
+        pidOutput = self.pidController.calculate(
+            self.encoder.getDistance(), self.setpoint.position
+        )
+        feedforwardOutput = self.feedforward.calculate(
+            self.setpoint.velocity, next_state.velocity
+        )
+
+        self.motor.setVoltage(pidOutput + feedforwardOutput)
+
+        self.setpoint = next_state
+
+    def stop(self) -> None:
+        """Stop the control loop and motor output."""
+        self.motor.setThrottle(0.0)
+
+    def reset(self) -> None:
+        """Reset Exponential profile to begin from current position on enable."""
+        self.setpoint = wpimath.ExponentialProfileMeterVolts.State(
+            self.encoder.getDistance(), 0
+        )
+
+    def updateTelemetry(self) -> None:
+        """Update telemetry, including the mechanism visualization."""
+        # Update elevator visualization with position
+        self.elevatorMech2d.setLength(self.encoder.getDistance())

robotpyExamples/examples/ElevatorProfiledPID/robot.py

@@ -0,0 +1,53 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+
+import wpilib
+import wpimath
+
+
+class MyRobot(wpilib.TimedRobot):
+    kDt = 0.02
+    kMaxVelocity = 1.75
+    kMaxAcceleration = 0.75
+    kP = 1.3
+    kI = 0.0
+    kD = 0.7
+    kS = 1.1
+    kG = 1.2
+    kV = 1.3
+
+    def __init__(self) -> None:
+        super().__init__()
+        self.joystick = wpilib.Joystick(1)
+        self.encoder = wpilib.Encoder(1, 2)
+        self.motor = wpilib.PWMSparkMax(1)
+
+        # Create a PID controller whose setpoint's change is subject to maximum
+        # velocity and acceleration constraints.
+        self.constraints = wpimath.TrapezoidProfile.Constraints(
+            self.kMaxVelocity, self.kMaxAcceleration
+        )
+        self.controller = wpimath.ProfiledPIDController(
+            self.kP, self.kI, self.kD, self.constraints, self.kDt
+        )
+        self.feedforward = wpimath.ElevatorFeedforward(self.kS, self.kG, self.kV)
+
+        self.encoder.setDistancePerPulse(1 / 360 * 2 * math.pi * 1.5)
+
+    def teleopPeriodic(self) -> None:
+        if self.joystick.getRawButtonPressed(2):
+            self.controller.setGoal(5)
+        elif self.joystick.getRawButtonPressed(3):
+            self.controller.setGoal(0)
+
+        # Run controller and update motor output
+        self.motor.setVoltage(
+            self.controller.calculate(self.encoder.getDistance())
+            + self.feedforward.calculate(self.controller.getSetpoint().velocity)
+        )

robotpyExamples/examples/ElevatorSimulation/constants.py

@@ -0,0 +1,36 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+
+import wpimath.units
+
+kMotorPort = 0
+kEncoderAChannel = 0
+kEncoderBChannel = 1
+kJoystickPort = 0
+
+kElevatorKp = 5.0
+kElevatorKi = 0.0
+kElevatorKd = 0.0
+
+kElevatorkS = 0.0  # volts (V)
+kElevatorkG = 0.762  # volts (V)
+kElevatorkV = 0.762  # volt per velocity (V/(m/s))
+kElevatorkA = 0.0  # volt per acceleration (V/(m/s^2))
+
+kElevatorGearing = 10.0
+kElevatorDrumRadius = wpimath.units.inchesToMeters(2.0)
+kCarriageMass = 4.0  # kg
+
+kSetpoint = 0.75  # m
+# Encoder is reset to measure 0 at the bottom, so minimum height is 0.
+kMinElevatorHeight = 0.0  # m
+kMaxElevatorHeight = 1.25  # m
+
+# distance per pulse = (distance per revolution) / (pulses per revolution)
+#  = (Pi * D) / ppr
+kElevatorEncoderDistPerPulse = 2.0 * math.pi * kElevatorDrumRadius / 4096

robotpyExamples/examples/ElevatorSimulation/robot.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+
+import constants
+from subsystems.elevator import Elevator
+
+
+class MyRobot(wpilib.TimedRobot):
+    """This is a sample program to demonstrate the use of elevator simulation."""
+
+    def __init__(self) -> None:
+        super().__init__()
+        self.joystick = wpilib.Joystick(constants.kJoystickPort)
+        self.elevator = Elevator()
+
+    def robotPeriodic(self) -> None:
+        # Update the telemetry, including mechanism visualization, regardless of mode.
+        self.elevator.updateTelemetry()
+
+    def simulationPeriodic(self) -> None:
+        # Update the simulation model.
+        self.elevator.simulationPeriodic()
+
+    def teleopPeriodic(self) -> None:
+        if self.joystick.getTrigger():
+            # Here, we set the constant setpoint of 0.75 meters.
+            self.elevator.reachGoal(constants.kSetpoint)
+        else:
+            # Otherwise, we update the setpoint to 0.
+            self.elevator.reachGoal(0.0)
+
+    def disabledInit(self) -> None:
+        # This just makes sure that our simulation code knows that the motor's off.
+        self.elevator.stop()

robotpyExamples/examples/ElevatorSimulation/subsystems/elevator.py

@@ -0,0 +1,107 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import wpilib.simulation
+import wpimath
+
+import constants
+
+
+class Elevator:
+    """Represents the elevator subsystem."""
+
+    def __init__(self) -> None:
+        # This gearbox represents a gearbox containing 4 Vex 775pro motors.
+        self.elevatorGearbox = wpimath.DCMotor.vex775Pro(4)
+
+        # Standard classes for controlling our elevator
+        self.controller = wpimath.ProfiledPIDController(
+            constants.kElevatorKp,
+            constants.kElevatorKi,
+            constants.kElevatorKd,
+            wpimath.TrapezoidProfile.Constraints(2.45, 2.45),
+        )
+        self.feedforward = wpimath.ElevatorFeedforward(
+            constants.kElevatorkS,
+            constants.kElevatorkG,
+            constants.kElevatorkV,
+            constants.kElevatorkA,
+        )
+        self.encoder = wpilib.Encoder(
+            constants.kEncoderAChannel, constants.kEncoderBChannel
+        )
+        self.motor = wpilib.PWMSparkMax(constants.kMotorPort)
+
+        # Simulation classes help us simulate what's going on, including gravity.
+        self.elevatorSim = wpilib.simulation.ElevatorSim(
+            self.elevatorGearbox,
+            constants.kElevatorGearing,
+            constants.kCarriageMass,
+            constants.kElevatorDrumRadius,
+            constants.kMinElevatorHeight,
+            constants.kMaxElevatorHeight,
+            True,
+            0,
+            [0.01, 0.0],
+        )
+        self.encoderSim = wpilib.simulation.EncoderSim(self.encoder)
+        self.motorSim = wpilib.simulation.PWMMotorControllerSim(self.motor)
+
+        # Create a Mechanism2d visualization of the elevator
+        self.mech2d = wpilib.Mechanism2d(20, 50)
+        self.mech2dRoot = self.mech2d.getRoot("Elevator Root", 10, 0)
+        self.elevatorMech2d = self.mech2dRoot.appendLigament(
+            "Elevator", self.elevatorSim.getPosition(), 90
+        )
+
+        self.encoder.setDistancePerPulse(constants.kElevatorEncoderDistPerPulse)
+
+        # Publish Mechanism2d to SmartDashboard
+        # To view the Elevator visualization, select Network Tables -> SmartDashboard -> Elevator Sim
+        wpilib.SmartDashboard.putData("Elevator Sim", self.mech2d)
+
+    def simulationPeriodic(self) -> None:
+        # In this method, we update our simulation of what our elevator is doing
+        # First, we set our "inputs" (voltages)
+        self.elevatorSim.setInputVoltage(
+            self.motorSim.getThrottle() * wpilib.RobotController.getBatteryVoltage()
+        )
+
+        # Next, we update it. The standard loop time is 20ms.
+        self.elevatorSim.update(0.020)
+
+        # Finally, we set our simulated encoder's readings and simulated battery voltage
+        self.encoderSim.setDistance(self.elevatorSim.getPosition())
+        # SimBattery estimates loaded battery voltages
+        wpilib.simulation.RoboRioSim.setVInVoltage(
+            wpilib.simulation.BatterySim.calculate([self.elevatorSim.getCurrentDraw()])
+        )
+
+    def reachGoal(self, goal: float) -> None:
+        """Run control loop to reach and maintain goal.
+
+        :param goal: the position to maintain
+        """
+
+        self.controller.setGoal(goal)
+
+        # With the setpoint value we run PID control like normal
+        pidOutput = self.controller.calculate(self.encoder.getDistance())
+        feedforwardOutput = self.feedforward.calculate(
+            self.controller.getSetpoint().velocity
+        )
+        self.motor.setVoltage(pidOutput + feedforwardOutput)
+
+    def stop(self) -> None:
+        """Stop the control loop and motor output."""
+        self.controller.setGoal(0.0)
+        self.motor.setThrottle(0.0)
+
+    def updateTelemetry(self) -> None:
+        """Update telemetry, including the mechanism visualization."""
+        # Update elevator visualization with position
+        self.elevatorMech2d.setLength(self.encoder.getDistance())

robotpyExamples/examples/ElevatorTrapezoidProfile/examplesmartmotorcontroller.py

@@ -0,0 +1,98 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import enum
+
+
+class ExampleSmartMotorController(wpilib.MotorController):
+    """A simplified stub class that simulates the API of a common "smart" motor controller.
+    Has no actual functionality.
+    """
+
+    class PIDMode(enum.Enum):
+        kPosition = enum.auto()
+        kVelocity = enum.auto()
+        kMovementWitchcraft = enum.auto()
+
+    def __init__(self, port: int) -> None:
+        """Creates a new ExampleSmartMotorController.
+
+        Args:
+            port: The port for the controller.
+        """
+        super().__init__()
+        self._velocity = 0.0
+        self._inverted = False
+        self._leader = None
+
+    def setPID(self, kp: float, ki: float, kd: float) -> None:
+        """Example method for setting the PID gains of the smart controller.
+
+        Args:
+            kp: The proportional gain.
+            ki: The integral gain.
+            kd: The derivative gain.
+        """
+        pass
+
+    def setSetPoint(
+        self, mode: PIDMode, setpoint: float, arbfeedforward: float
+    ) -> None:
+        """Example method for setting the setpoint of the smart controller in PID mode.
+
+        Args:
+            mode: The mode of the PID controller.
+            setpoint: The controller setpoint.
+            arbfeedforward: An arbitrary feedforward output (from -1 to 1).
+        """
+        pass
+
+    def follow(self, leader: "ExampleSmartMotorController") -> None:
+        """Places this motor controller in follower mode.
+
+        Args:
+            leader: The leader to follow.
+        """
+        self._leader = leader
+
+    def getEncoderDistance(self) -> float:
+        """Returns the encoder distance.
+
+        Returns:
+            The current encoder distance.
+        """
+        return 0
+
+    def getEncoderRate(self) -> float:
+        """Returns the encoder rate.
+
+        Returns:
+            The current encoder rate.
+        """
+        return 0
+
+    def resetEncoder(self) -> None:
+        """Resets the encoder to zero distance."""
+        pass
+
+    def set(self, velocity: float) -> None:
+        self._velocity = -velocity if self._inverted else velocity
+
+    def get(self) -> float:
+        return self._velocity
+
+    def setInverted(self, isInverted: bool) -> None:
+        self._inverted = isInverted
+
+    def getInverted(self) -> bool:
+        return self._inverted
+
+    def disable(self) -> None:
+        self._velocity = 0.0
+
+    def stopMotor(self) -> None:
+        self._velocity = 0.0

robotpyExamples/examples/ElevatorTrapezoidProfile/robot.py

@@ -0,0 +1,50 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+from wpimath import TrapezoidProfile
+
+import examplesmartmotorcontroller
+import wpimath
+
+
+class MyRobot(wpilib.TimedRobot):
+    kDt = 0.02
+
+    def __init__(self):
+        super().__init__()
+        self.joystick = wpilib.Joystick(1)
+        self.motor = examplesmartmotorcontroller.ExampleSmartMotorController(1)
+        # Note: These gains are fake, and will have to be tuned for your robot.
+        self.feedforward = wpimath.SimpleMotorFeedforwardMeters(1, 1.5)
+
+        # Create a motion profile with the given maximum velocity and maximum
+        # acceleration constraints for the next setpoint.
+        self.profile = TrapezoidProfile(TrapezoidProfile.Constraints(1.75, 0.75))
+
+        self.goal = TrapezoidProfile.State()
+        self.setpoint = TrapezoidProfile.State()
+
+        # Note: These gains are fake, and will have to be tuned for your robot.
+        self.motor.setPID(1.3, 0.0, 0.7)
+
+    def teleopPeriodic(self):
+        if self.joystick.getRawButtonPressed(2):
+            self.goal = TrapezoidProfile.State(5, 0)
+        elif self.joystick.getRawButtonPressed(3):
+            self.goal = TrapezoidProfile.State(0, 0)
+
+        # Retrieve the profiled setpoint for the next timestep. This setpoint moves
+        # toward the goal while obeying the constraints.
+        self.setpoint = self.profile.calculate(self.kDt, self.setpoint, self.goal)
+
+        # Send setpoint to offboard controller PID
+        self.motor.setSetPoint(
+            examplesmartmotorcontroller.ExampleSmartMotorController.PIDMode.kPosition,
+            self.setpoint.position,
+            self.feedforward.calculate(self.setpoint.velocity) / 12,
+        )

robotpyExamples/examples/Encoder/robot.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+
+import math
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    Sample program displaying the value of a quadrature encoder on the SmartDashboard. Quadrature
+    Encoders are digital sensors which can detect the amount the encoder has rotated since starting
+    as well as the direction in which the encoder shaft is rotating. However, encoders can not tell
+    you the absolute position of the encoder shaft (ie, it considers where it starts to be the zero
+    position, no matter where it starts), and so can only tell you how much the encoder has rotated
+    since starting. Depending on the precision of an encoder, it will have fewer or greater ticks per
+    revolution; the number of ticks per revolution will affect the conversion between ticks and
+    distance, as specified by DistancePerPulse. One of the most common uses of encoders is in the
+    drivetrain, so that the distance that the robot drives can be precisely controlled during the
+    autonomous mode.
+    """
+
+    def __init__(self):
+        """Robot initialization function"""
+        super().__init__()
+
+        self.encoder = wpilib.Encoder(1, 2, False, wpilib.Encoder.EncodingType.X4)
+
+        # Defines the number of samples to average when determining the rate.
+        # On a quadrature encoder, values range from 1-255;
+        # larger values result in smoother but potentially
+        # less accurate rates than lower values.
+        self.encoder.setSamplesToAverage(5)
+
+        # Defines how far the mechanism attached to the encoder moves per pulse. In
+        # this case, we assume that a 360 count encoder is directly
+        # attached to a 3 inch diameter (1.5inch radius) wheel,
+        # and that we want to measure distance in inches.
+        self.encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * math.pi * 1.5)
+
+        # Defines the lowest rate at which the encoder will
+        # not be considered stopped, for the purposes of
+        # the GetStopped() method. Units are in distance / second,
+        # where distance refers to the units of distance
+        # that you are using, in this case inches.
+        self.encoder.setMinRate(1.0)
+
+    def teleopPeriodic(self):
+        wpilib.SmartDashboard.putNumber("Encoder Distance", self.encoder.getDistance())
+        wpilib.SmartDashboard.putNumber("Encoder Rate", self.encoder.getRate())

robotpyExamples/examples/GettingStarted/robot.py

@@ -0,0 +1,56 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+
+
+class MyRobot(wpilib.TimedRobot):
+    def __init__(self):
+        """
+        This function is called upon program startup and
+        should be used for any initialization code.
+        """
+        super().__init__()
+        self.leftDrive = wpilib.PWMSparkMax(0)
+        self.rightDrive = wpilib.PWMSparkMax(1)
+        self.robotDrive = wpilib.DifferentialDrive(self.leftDrive, self.rightDrive)
+        self.controller = wpilib.NiDsXboxController(0)
+        self.timer = wpilib.Timer()
+
+        # We need to invert one side of the drivetrain so that positive voltages
+        # result in both sides moving forward. Depending on how your robot's
+        # gearbox is constructed, you might have to invert the left side instead.
+        self.rightDrive.setInverted(True)
+
+    def autonomousInit(self):
+        """This function is run once each time the robot enters autonomous mode."""
+        self.timer.restart()
+
+    def autonomousPeriodic(self):
+        """This function is called periodically during autonomous."""
+
+        # Drive for two seconds
+        if self.timer.get() < 2.0:
+            # Drive forwards half velocity, make sure to turn input squaring off
+            self.robotDrive.arcadeDrive(0.5, 0, squareInputs=False)
+        else:
+            self.robotDrive.stopMotor()  # Stop robot
+
+    def teleopInit(self):
+        """This function is called once each time the robot enters teleoperated mode."""
+
+    def teleopPeriodic(self):
+        """This function is called periodically during teleoperated mode."""
+        self.robotDrive.arcadeDrive(
+            -self.controller.getLeftY(), -self.controller.getRightX()
+        )
+
+    def utilityInit(self):
+        """This function is called once each time the robot enters utility mode."""
+
+    def utilityPeriodic(self):
+        """This function is called periodically during utility mode."""

robotpyExamples/examples/Gyro/robot.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    This is a sample program to demonstrate how to use a gyro sensor to make a robot drive straight.
+    This program uses a joystick to drive forwards and backwards while the gyro is used for direction
+    keeping.
+    """
+
+    kAngleSetpoint = 0.0
+    kP = 0.005  # proportional turning constant
+
+    kLeftMotorPort = 0
+    kRightMotorPort = 1
+    kIMUMountOrientation = wpilib.OnboardIMU.MountOrientation.FLAT
+    kJoystickPort = 0
+
+    def __init__(self) -> None:
+        """Robot initialization function"""
+        super().__init__()
+
+        self.leftDrive = wpilib.PWMSparkMax(self.kLeftMotorPort)
+        self.rightDrive = wpilib.PWMSparkMax(self.kRightMotorPort)
+        self.myRobot = wpilib.DifferentialDrive(self.leftDrive, self.rightDrive)
+        self.imu = wpilib.OnboardIMU(self.kIMUMountOrientation)
+        self.joystick = wpilib.Joystick(self.kJoystickPort)
+
+        # We need to invert one side of the drivetrain so that positive voltages
+        # result in both sides moving forward. Depending on how your robot's
+        # gearbox is constructed, you might have to invert the left side instead.
+        self.rightDrive.setInverted(True)
+
+    def teleopPeriodic(self) -> None:
+        # The motor velocity is set from the joystick while the DifferentialDrive turning value is assigned
+        # from the error between the setpoint and the gyro angle.
+        turningValue = (
+            self.kAngleSetpoint - self.imu.getRotation2d().degrees()
+        ) * self.kP
+        self.myRobot.arcadeDrive(-self.joystick.getY(), -turningValue)

robotpyExamples/examples/HatchbotInlined/commands/autos.py

@@ -0,0 +1,75 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+import commands2.cmd
+
+import subsystems.drivesubsystem
+import subsystems.hatchsubsystem
+
+import constants
+
+
+class Autos:
+    """Container for auto command factories."""
+
+    def __init__(self) -> None:
+        raise Exception("This is a utility class!")
+
+    @staticmethod
+    def simpleAuto(drive: subsystems.drivesubsystem.DriveSubsystem):
+        """A simple auto routine that drives forward a specified distance, and then stops."""
+        return commands2.FunctionalCommand(
+            # Reset encoders on command start
+            drive.resetEncoders,
+            # Drive forward while the command is executing,
+            lambda: drive.arcadeDrive(constants.kAutoDriveVelocity, 0),
+            # Stop driving at the end of the command
+            lambda interrupt: drive.arcadeDrive(0, 0),
+            # End the command when the robot's driven distance exceeds the desired value
+            lambda: drive.getAverageEncoderDistance()
+            >= constants.kAutoDriveDistanceInches,
+            # Require the drive subsystem
+        )
+
+    @staticmethod
+    def complexAuto(
+        driveSubsystem: subsystems.drivesubsystem.DriveSubsystem,
+        hatchSubsystem: subsystems.hatchsubsystem.HatchSubsystem,
+    ):
+        """A complex auto routine that drives forward, drops a hatch, and then drives backward."""
+        return commands2.cmd.sequence(
+            # Drive forward up to the front of the cargo ship
+            commands2.FunctionalCommand(
+                # Reset encoders on command start
+                driveSubsystem.resetEncoders,
+                # Drive forward while the command is executing
+                lambda: driveSubsystem.arcadeDrive(constants.kAutoDriveVelocity, 0),
+                # Stop driving at the end of the command
+                lambda interrupt: driveSubsystem.arcadeDrive(0, 0),
+                # End the command when the robot's driven distance exceeds the desired value
+                lambda: driveSubsystem.getAverageEncoderDistance()
+                >= constants.kAutoDriveDistanceInches,
+                # Require the drive subsystem
+                driveSubsystem,
+            ),
+            # Release the hatch
+            hatchSubsystem.releaseHatch(),
+            # Drive backward the specified distance
+            commands2.FunctionalCommand(
+                # Reset encoders on command start
+                driveSubsystem.resetEncoders,
+                # Drive backwards while the command is executing
+                lambda: driveSubsystem.arcadeDrive(-constants.kAutoDriveVelocity, 0),
+                # Stop driving at the end of the command
+                lambda interrupt: driveSubsystem.arcadeDrive(0, 0),
+                # End the command when the robot's driven distance exceeds the desired value
+                lambda: abs(driveSubsystem.getAverageEncoderDistance())
+                >= constants.kAutoBackupDistanceInches,
+                # Require the drive subsystem
+                driveSubsystem,
+            ),
+        )

robotpyExamples/examples/HatchbotInlined/constants.py

@@ -0,0 +1,51 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+#
+# The constants module is a convenience place for teams to hold robot-wide
+# numerical or boolean constants. Don't use this for any other purpose!
+#
+
+import math
+import wpilib
+
+# Motors
+kLeftMotor1Port = 0
+kLeftMotor2Port = 1
+kRightMotor1Port = 2
+kRightMotor2Port = 3
+
+# Encoders
+kLeftEncoderPorts = (0, 1)
+kRightEncoderPorts = (2, 3)
+kLeftEncoderReversed = False
+kRightEncoderReversed = True
+
+kEncoderCPR = 1024
+kWheelDiameterInches = 6
+# Assumes the encoders are directly mounted on the wheel shafts
+kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR
+
+# Hatch
+kHatchSolenoidModuleType = wpilib.PneumaticsModuleType.CTRE_PCM
+kHatchSolenoidModule = 0
+kHatchSolenoidPorts = (0, 1)
+
+# Autonomous
+kAutoDriveDistanceInches = 60
+kAutoBackupDistanceInches = 20
+kAutoDriveVelocity = 0.5
+
+# Operator Interface
+kDriverControllerPort = 0
+
+# Physical parameters
+kDriveTrainMotorCount = 2
+kTrackWidth = 0.381 * 2
+kGearingRatio = 8
+kWheelRadius = 0.0508
+
+# kEncoderResolution = -

robotpyExamples/examples/HatchbotInlined/robot.py

@@ -0,0 +1,71 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import typing
+
+import commands2
+import wpilib
+
+from robotcontainer import RobotContainer
+
+
+class MyRobot(commands2.TimedCommandRobot):
+    """
+    Command v2 robots are encouraged to inherit from TimedCommandRobot, which
+    has an implementation of robotPeriodic which runs the scheduler for you
+    """
+
+    autonomousCommand: typing.Optional[commands2.Command] = None
+
+    def __init__(self) -> None:
+        """
+        This function is run when the robot is first started up and should be used for any
+        initialization code.
+        """
+        super().__init__()
+
+        # Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+        # autonomous chooser on the dashboard.
+        self.container = RobotContainer()
+
+        # Start recording to data log
+        wpilib.DataLogManager.start()
+
+        # Record DS control and joystick data.
+        # Change to `false` to not record joystick data.
+        wpilib.DriverStation.startDataLog(wpilib.DataLogManager.getLog(), True)
+
+    def disabledInit(self) -> None:
+        """This function is called once each time the robot enters Disabled mode."""
+
+    def disabledPeriodic(self) -> None:
+        """This function is called periodically when disabled"""
+
+    def autonomousInit(self) -> None:
+        """This autonomous runs the autonomous command selected by your RobotContainer class."""
+        self.autonomousCommand = self.container.getAutonomousCommand()
+
+        if self.autonomousCommand:
+            self.autonomousCommand.schedule()
+
+    def autonomousPeriodic(self) -> None:
+        """This function is called periodically during autonomous"""
+
+    def teleopInit(self) -> None:
+        # This makes sure that the autonomous stops running when
+        # teleop starts running. If you want the autonomous to
+        # continue until interrupted by another command, remove
+        # this line or comment it out.
+        if self.autonomousCommand:
+            self.autonomousCommand.cancel()
+
+    def teleopPeriodic(self) -> None:
+        """This function is called periodically during operator control"""
+
+    def utilityInit(self) -> None:
+        # Cancels all running commands at the start of utility mode
+        commands2.CommandScheduler.getInstance().cancelAll()

robotpyExamples/examples/HatchbotInlined/robotcontainer.py

@@ -0,0 +1,95 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+
+import commands2
+import commands2.button
+import commands2.cmd
+
+import constants
+
+import commands.autos
+
+from subsystems.drivesubsystem import DriveSubsystem
+from subsystems.hatchsubsystem import HatchSubsystem
+
+
+class RobotContainer:
+    """
+    This class is where the bulk of the robot should be declared. Since Command-based is a
+    "declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot`
+    periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+    subsystems, commands, and button mappings) should be declared here.
+    """
+
+    def __init__(self) -> None:
+        # The robot's subsystems
+        self.driveSubsystem = DriveSubsystem()
+        self.hatchSubsystem = HatchSubsystem()
+
+        # Retained command handles
+
+        # A simple auto routine that drives forward a specified distance, and then stops.
+        self.simpleAuto = commands.autos.Autos.simpleAuto(self.driveSubsystem)
+
+        # A complex auto routine that drives forward, drops a hatch, and then drives backward.
+        self.complexAuto = commands.autos.Autos.complexAuto(
+            self.driveSubsystem, self.hatchSubsystem
+        )
+
+        # The driver's controller
+        self.driverController = commands2.button.CommandNiDsPS4Controller(
+            constants.kDriverControllerPort
+        )
+
+        # Configure the button bindings
+        self.configureButtonBindings()
+
+        # Configure default commands
+        # Set the default drive command to split-stick arcade drive
+        self.driveSubsystem.setDefaultCommand(
+            # A split-stick arcade command, with forward/backward controlled by the left
+            # hand, and turning controlled by the right.
+            commands2.cmd.run(
+                lambda: self.driveSubsystem.arcadeDrive(
+                    -self.driverController.getLeftY(),
+                    -self.driverController.getRightX(),
+                ),
+                self.driveSubsystem,
+            )
+        )
+
+        # Chooser
+        self.chooser = wpilib.SendableChooser()
+
+        # Add commands to the autonomous command chooser
+        self.chooser.setDefaultOption("Simple Auto", self.simpleAuto)
+        self.chooser.addOption("Complex Auto", self.complexAuto)
+
+        # Put the chooser on the dashboard
+        wpilib.SmartDashboard.putData("Autonomous", self.chooser)
+
+    def configureButtonBindings(self):
+        """
+        Use this method to define your button->command mappings. Buttons can be created by
+        instantiating a wpilib.GenericHID or one of its subclasses (Joystick or XboxController),
+        and then passing it to a JoystickButton.
+        """
+
+        # Grab the hatch when the Circle button is pressed.
+        self.driverController.circle().onTrue(self.hatchSubsystem.grabHatch())
+
+        # Release the hatch when the Square button is pressed.
+        self.driverController.square().onTrue(self.hatchSubsystem.releaseHatch())
+
+        # While holding R1, drive at half velocity
+        self.driverController.R1().onTrue(
+            commands2.cmd.runOnce(lambda: self.driveSubsystem.setMaxOutput(0.5))
+        ).onFalse(commands2.cmd.runOnce(lambda: self.driveSubsystem.setMaxOutput(1)))
+
+    def getAutonomousCommand(self) -> commands2.Command:
+        return self.chooser.getSelected()

robotpyExamples/examples/HatchbotInlined/subsystems/drivesubsystem.py

@@ -0,0 +1,68 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+import wpilib
+import constants
+
+
+class DriveSubsystem(commands2.Subsystem):
+    def __init__(self) -> None:
+
+        self.left1 = wpilib.PWMVictorSPX(constants.kLeftMotor1Port)
+        self.left2 = wpilib.PWMVictorSPX(constants.kLeftMotor2Port)
+        self.right1 = wpilib.PWMVictorSPX(constants.kRightMotor1Port)
+        self.right2 = wpilib.PWMVictorSPX(constants.kRightMotor2Port)
+
+        self.left1.addFollower(self.left2)
+        self.right1.addFollower(self.right2)
+
+        # We need to invert one side of the drivetrain so that positive velocities
+        # result in both sides moving forward. Depending on how your robot's
+        # drivetrain is constructed, you might have to invert the left side instead.
+        self.right1.setInverted(True)
+
+        # The robot's drive
+        self.drive = wpilib.DifferentialDrive(self.left1, self.right1)
+
+        # The left-side drive encoder
+        self.leftEncoder = wpilib.Encoder(
+            *constants.kLeftEncoderPorts,
+            reverseDirection=constants.kLeftEncoderReversed
+        )
+
+        # The right-side drive encoder
+        self.rightEncoder = wpilib.Encoder(
+            *constants.kRightEncoderPorts,
+            reverseDirection=constants.kRightEncoderReversed
+        )
+
+        # Sets the distance per pulse for the encoders
+        self.leftEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse)
+        self.rightEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse)
+
+    def arcadeDrive(self, fwd: float, rot: float) -> None:
+        """
+        Drives the robot using arcade controls.
+
+        :param fwd: the commanded forward movement
+        :param rot: the commanded rotation
+        """
+        self.drive.arcadeDrive(fwd, rot)
+
+    def resetEncoders(self) -> None:
+        """Resets the drive encoders to currently read a position of 0."""
+
+    def getAverageEncoderDistance(self) -> float:
+        """Gets the average distance of the TWO encoders."""
+        return (self.leftEncoder.getDistance() + self.rightEncoder.getDistance()) / 2.0
+
+    def setMaxOutput(self, maxOutput: float):
+        """
+        Sets the max output of the drive. Useful for scaling the
+        drive to drive more slowly.
+        """
+        self.drive.setMaxOutput(maxOutput)

robotpyExamples/examples/HatchbotInlined/subsystems/hatchsubsystem.py

@@ -0,0 +1,33 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import commands2
+import commands2.cmd
+
+import constants
+
+
+class HatchSubsystem(commands2.Subsystem):
+    def __init__(self) -> None:
+
+        self.hatchSolenoid = wpilib.DoubleSolenoid(
+            constants.kHatchSolenoidModule,
+            constants.kHatchSolenoidModuleType,
+            *constants.kHatchSolenoidPorts
+        )
+
+    def grabHatch(self) -> commands2.Command:
+        """Grabs the hatch"""
+        return commands2.cmd.runOnce(
+            lambda: self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.FORWARD), self
+        )
+
+    def releaseHatch(self) -> commands2.Command:
+        """Releases the hatch"""
+        return commands2.cmd.runOnce(
+            lambda: self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.REVERSE), self
+        )

robotpyExamples/examples/HatchbotTraditional/commands/complexauto.py

@@ -0,0 +1,37 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+
+import constants
+
+from .drivedistance import DriveDistance
+from .releasehatch import ReleaseHatch
+
+from subsystems.drivesubsystem import DriveSubsystem
+from subsystems.hatchsubsystem import HatchSubsystem
+
+
+class ComplexAuto(commands2.SequentialCommandGroup):
+    """
+    A complex auto command that drives forward, releases a hatch, and then drives backward.
+    """
+
+    def __init__(self, drive: DriveSubsystem, hatch: HatchSubsystem):
+        super().__init__(
+            # Drive forward the specified distance
+            DriveDistance(
+                constants.kAutoDriveDistanceInches, constants.kAutoDriveVelocity, drive
+            ),
+            # Release the hatch
+            ReleaseHatch(hatch),
+            # Drive backward the specified distance
+            DriveDistance(
+                constants.kAutoBackupDistanceInches,
+                -constants.kAutoDriveVelocity,
+                drive,
+            ),
+        )

robotpyExamples/examples/HatchbotTraditional/commands/defaultdrive.py

@@ -0,0 +1,27 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import typing
+import commands2
+from subsystems.drivesubsystem import DriveSubsystem
+
+
+class DefaultDrive(commands2.Command):
+    def __init__(
+        self,
+        drive: DriveSubsystem,
+        forward: typing.Callable[[], float],
+        rotation: typing.Callable[[], float],
+    ) -> None:
+
+        self.drive = drive
+        self.forward = forward
+        self.rotation = rotation
+
+        self.addRequirements(self.drive)
+
+    def execute(self) -> None:
+        self.drive.arcadeDrive(self.forward(), self.rotation())

robotpyExamples/examples/HatchbotTraditional/commands/drivedistance.py

@@ -0,0 +1,30 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+
+from subsystems.drivesubsystem import DriveSubsystem
+
+
+class DriveDistance(commands2.Command):
+    def __init__(self, inches: float, velocity: float, drive: DriveSubsystem) -> None:
+        self.distance = inches
+        self.velocity = velocity
+        self.drive = drive
+        self.addRequirements(drive)
+
+    def initialize(self) -> None:
+        self.drive.resetEncoders()
+        self.drive.arcadeDrive(self.velocity, 0)
+
+    def execute(self) -> None:
+        self.drive.arcadeDrive(self.velocity, 0)
+
+    def end(self, interrupted: bool) -> None:
+        self.drive.arcadeDrive(0, 0)
+
+    def isFinished(self) -> bool:
+        return abs(self.drive.getAverageEncoderDistance()) >= self.distance

robotpyExamples/examples/HatchbotTraditional/commands/grabhatch.py

@@ -0,0 +1,20 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+from subsystems.hatchsubsystem import HatchSubsystem
+
+
+class GrabHatch(commands2.Command):
+    def __init__(self, hatch: HatchSubsystem) -> None:
+        self.hatch = hatch
+        self.addRequirements(hatch)
+
+    def initialize(self) -> None:
+        self.hatch.grabHatch()
+
+    def isFinished(self) -> bool:
+        return True

robotpyExamples/examples/HatchbotTraditional/commands/halvedrivevelocity.py

@@ -0,0 +1,20 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+
+from subsystems.drivesubsystem import DriveSubsystem
+
+
+class HalveDriveVelocity(commands2.Command):
+    def __init__(self, drive: DriveSubsystem) -> None:
+        self.drive = drive
+
+    def initialize(self) -> None:
+        self.drive.setMaxOutput(0.5)
+
+    def end(self, interrupted: bool) -> None:
+        self.drive.setMaxOutput(1.0)

robotpyExamples/examples/HatchbotTraditional/commands/releasehatch.py

@@ -0,0 +1,14 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+
+from subsystems.hatchsubsystem import HatchSubsystem
+
+
+class ReleaseHatch(commands2.InstantCommand):
+    def __init__(self, hatch: HatchSubsystem) -> None:
+        super().__init__(hatch.releaseHatch, hatch)

robotpyExamples/examples/HatchbotTraditional/constants.py

@@ -0,0 +1,51 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+#
+# The constants module is a convenience place for teams to hold robot-wide
+# numerical or boolean constants. Don't use this for any other purpose!
+#
+
+import math
+import wpilib
+
+# Motors
+kLeftMotor1Port = 0
+kLeftMotor2Port = 1
+kRightMotor1Port = 2
+kRightMotor2Port = 3
+
+# Encoders
+kLeftEncoderPorts = (0, 1)
+kRightEncoderPorts = (2, 3)
+kLeftEncoderReversed = False
+kRightEncoderReversed = True
+
+kEncoderCPR = 1024
+kWheelDiameterInches = 6
+# Assumes the encoders are directly mounted on the wheel shafts
+kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR
+
+# Hatch
+kHatchSolenoidModuleType = wpilib.PneumaticsModuleType.CTRE_PCM
+kHatchSolenoidModule = 0
+kHatchSolenoidPorts = (0, 1)
+
+# Autonomous
+kAutoDriveDistanceInches = 60
+kAutoBackupDistanceInches = 20
+kAutoDriveVelocity = 0.5
+
+# Operator Interface
+kDriverControllerPort = 0
+
+# Physical parameters
+kDriveTrainMotorCount = 2
+kTrackWidth = 0.381 * 2
+kGearingRatio = 8
+kWheelRadius = 0.0508
+
+# kEncoderResolution = -

robotpyExamples/examples/HatchbotTraditional/robot.py

@@ -0,0 +1,71 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import typing
+
+import commands2
+import wpilib
+
+from robotcontainer import RobotContainer
+
+
+class MyRobot(commands2.TimedCommandRobot):
+    """
+    Command v2 robots are encouraged to inherit from TimedCommandRobot, which
+    has an implementation of robotPeriodic which runs the scheduler for you
+    """
+
+    autonomousCommand: typing.Optional[commands2.Command] = None
+
+    def __init__(self) -> None:
+        """
+        This function is run when the robot is first started up and should be used for any
+        initialization code.
+        """
+        super().__init__()
+
+        # Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+        # autonomous chooser on the dashboard.
+        self.container = RobotContainer()
+
+        # Start recording to data log
+        wpilib.DataLogManager.start()
+
+        # Record DS control and joystick data.
+        # Change to `false` to not record joystick data.
+        wpilib.DriverStation.startDataLog(wpilib.DataLogManager.getLog(), True)
+
+    def disabledInit(self) -> None:
+        """This function is called once each time the robot enters Disabled mode."""
+
+    def disabledPeriodic(self) -> None:
+        """This function is called periodically when disabled"""
+
+    def autonomousInit(self) -> None:
+        """This autonomous runs the autonomous command selected by your RobotContainer class."""
+        self.autonomousCommand = self.container.getAutonomousCommand()
+
+        if self.autonomousCommand:
+            self.autonomousCommand.schedule()
+
+    def autonomousPeriodic(self) -> None:
+        """This function is called periodically during autonomous"""
+
+    def teleopInit(self) -> None:
+        # This makes sure that the autonomous stops running when
+        # teleop starts running. If you want the autonomous to
+        # continue until interrupted by another command, remove
+        # this line or comment it out.
+        if self.autonomousCommand:
+            self.autonomousCommand.cancel()
+
+    def teleopPeriodic(self) -> None:
+        """This function is called periodically during operator control"""
+
+    def utilityInit(self) -> None:
+        # Cancels all running commands at the start of utility mode
+        commands2.CommandScheduler.getInstance().cancelAll()

robotpyExamples/examples/HatchbotTraditional/robotcontainer.py

@@ -0,0 +1,93 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+
+import commands2
+import commands2.button
+
+import constants
+
+from commands.complexauto import ComplexAuto
+from commands.drivedistance import DriveDistance
+from commands.defaultdrive import DefaultDrive
+from commands.grabhatch import GrabHatch
+from commands.halvedrivevelocity import HalveDriveVelocity
+from commands.releasehatch import ReleaseHatch
+
+from subsystems.drivesubsystem import DriveSubsystem
+from subsystems.hatchsubsystem import HatchSubsystem
+
+
+class RobotContainer:
+    """
+    This class is where the bulk of the robot should be declared. Since Command-based is a
+    "declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot`
+    periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+    subsystems, commands, and button mappings) should be declared here.
+    """
+
+    def __init__(self) -> None:
+        # The driver's controller
+        # self.driverController = wpilib.NiDsXboxController(constants.kDriverControllerPort)
+        self.driverController = wpilib.Joystick(constants.kDriverControllerPort)
+
+        # The robot's subsystems
+        self.drive = DriveSubsystem()
+        self.hatch = HatchSubsystem()
+
+        # Autonomous routines
+
+        # A simple auto routine that drives forward a specified distance, and then stops.
+        self.simpleAuto = DriveDistance(
+            constants.kAutoDriveDistanceInches, constants.kAutoDriveVelocity, self.drive
+        )
+
+        # A complex auto routine that drives forward, drops a hatch, and then drives backward.
+        self.complexAuto = ComplexAuto(self.drive, self.hatch)
+
+        # Chooser
+        self.chooser = wpilib.SendableChooser()
+
+        # Add commands to the autonomous command chooser
+        self.chooser.setDefaultOption("Simple Auto", self.simpleAuto)
+        self.chooser.addOption("Complex Auto", self.complexAuto)
+
+        # Put the chooser on the dashboard
+        wpilib.SmartDashboard.putData("Autonomous", self.chooser)
+
+        self.configureButtonBindings()
+
+        # set up default drive command
+        self.drive.setDefaultCommand(
+            DefaultDrive(
+                self.drive,
+                lambda: -self.driverController.getY(),
+                lambda: self.driverController.getX(),
+            )
+        )
+
+    def configureButtonBindings(self):
+        """
+        Use this method to define your button->command mappings. Buttons can be created by
+        instantiating a wpilib.GenericHID or one of its subclasses (Joystick or XboxController),
+        and then passing it to a JoystickButton.
+        """
+
+        commands2.button.JoystickButton(self.driverController, 1).onTrue(
+            GrabHatch(self.hatch)
+        )
+
+        commands2.button.JoystickButton(self.driverController, 2).onTrue(
+            ReleaseHatch(self.hatch)
+        )
+
+        commands2.button.JoystickButton(self.driverController, 3).whileTrue(
+            HalveDriveVelocity(self.drive)
+        )
+
+    def getAutonomousCommand(self) -> commands2.Command:
+        return self.chooser.getSelected()

robotpyExamples/examples/HatchbotTraditional/subsystems/drivesubsystem.py

@@ -0,0 +1,65 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+import wpilib
+import constants
+
+
+class DriveSubsystem(commands2.Subsystem):
+    def __init__(self) -> None:
+
+        self.left1 = wpilib.PWMVictorSPX(constants.kLeftMotor1Port)
+        self.left2 = wpilib.PWMVictorSPX(constants.kLeftMotor2Port)
+        self.right1 = wpilib.PWMVictorSPX(constants.kRightMotor1Port)
+        self.right2 = wpilib.PWMVictorSPX(constants.kRightMotor2Port)
+
+        # We need to invert one side of the drivetrain so that positive velocities
+        # result in both sides moving forward. Depending on how your robot's
+        # drivetrain is constructed, you might have to invert the left side instead.
+        self.right1.setInverted(True)
+
+        # The robot's drive
+        self.drive = wpilib.DifferentialDrive(self.left1, self.right1)
+
+        # The left-side drive encoder
+        self.leftEncoder = wpilib.Encoder(
+            *constants.kLeftEncoderPorts,
+            reverseDirection=constants.kLeftEncoderReversed
+        )
+
+        # The right-side drive encoder
+        self.rightEncoder = wpilib.Encoder(
+            *constants.kRightEncoderPorts,
+            reverseDirection=constants.kRightEncoderReversed
+        )
+
+        # Sets the distance per pulse for the encoders
+        self.leftEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse)
+        self.rightEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse)
+
+    def arcadeDrive(self, fwd: float, rot: float) -> None:
+        """
+        Drives the robot using arcade controls.
+
+        :param fwd: the commanded forward movement
+        :param rot: the commanded rotation
+        """
+        self.drive.arcadeDrive(fwd, rot)
+
+    def resetEncoders(self) -> None:
+        """Resets the drive encoders to currently read a position of 0."""
+
+    def getAverageEncoderDistance(self) -> float:
+        """Gets the average distance of the TWO encoders."""
+        return (self.leftEncoder.getDistance() + self.rightEncoder.getDistance()) / 2.0
+
+    def setMaxOutput(self, maxOutput: float):
+        """
+        Sets the max output of the drive. Useful for scaling the
+        drive to drive more slowly.
+        """
+        self.drive.setMaxOutput(maxOutput)

robotpyExamples/examples/HatchbotTraditional/subsystems/hatchsubsystem.py

@@ -0,0 +1,28 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import commands2
+
+import constants
+
+
+class HatchSubsystem(commands2.Subsystem):
+    def __init__(self) -> None:
+
+        self.hatchSolenoid = wpilib.DoubleSolenoid(
+            constants.kHatchSolenoidModule,
+            constants.kHatchSolenoidModuleType,
+            *constants.kHatchSolenoidPorts
+        )
+
+    def grabHatch(self) -> None:
+        """Grabs the hatch"""
+        self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.FORWARD)
+
+    def releaseHatch(self) -> None:
+        """Releases the hatch"""
+        self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.REVERSE)

robotpyExamples/examples/MecanumBot/drivetrain.py

@@ -0,0 +1,129 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+import math
+
+import wpilib
+import wpimath
+
+
+class Drivetrain:
+    """Represents a mecanum drive style drivetrain."""
+
+    MAX_VELOCITY = 3.0  # 3 meters per second
+    MAX_ANGULAR_VELOCITY = math.pi  # 1/2 rotation per second
+
+    def __init__(self) -> None:
+        self.frontLeftMotor = wpilib.PWMSparkMax(1)
+        self.frontRightMotor = wpilib.PWMSparkMax(2)
+        self.backLeftMotor = wpilib.PWMSparkMax(3)
+        self.backRightMotor = wpilib.PWMSparkMax(4)
+
+        self.frontLeftEncoder = wpilib.Encoder(0, 1)
+        self.frontRightEncoder = wpilib.Encoder(2, 3)
+        self.backLeftEncoder = wpilib.Encoder(4, 5)
+        self.backRightEncoder = wpilib.Encoder(6, 7)
+
+        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.frontLeftPIDController = wpimath.PIDController(1, 0, 0)
+        self.frontRightPIDController = wpimath.PIDController(1, 0, 0)
+        self.backLeftPIDController = wpimath.PIDController(1, 0, 0)
+        self.backRightPIDController = wpimath.PIDController(1, 0, 0)
+
+        self.imu = wpilib.OnboardIMU(wpilib.OnboardIMU.MountOrientation.FLAT)
+
+        self.kinematics = wpimath.MecanumDriveKinematics(
+            self.frontLeftLocation,
+            self.frontRightLocation,
+            self.backLeftLocation,
+            self.backRightLocation,
+        )
+
+        self.odometry = wpimath.MecanumDriveOdometry(
+            self.kinematics, self.imu.getRotation2d(), self.getCurrentDistances()
+        )
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.feedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3)
+
+        self.imu.resetYaw()
+
+        # We need to invert one side of the drivetrain so that positive voltages
+        # result in both sides moving forward. Depending on how your robot's
+        # gearbox is constructed, you might have to invert the left side instead.
+        self.frontRightMotor.setInverted(True)
+        self.backRightMotor.setInverted(True)
+
+    def getCurrentState(self) -> wpimath.MecanumDriveWheelVelocities:
+        """Returns the current state of the drivetrain."""
+        return wpimath.MecanumDriveWheelVelocities(
+            self.frontLeftEncoder.getRate(),
+            self.frontRightEncoder.getRate(),
+            self.backLeftEncoder.getRate(),
+            self.backRightEncoder.getRate(),
+        )
+
+    def getCurrentDistances(self) -> wpimath.MecanumDriveWheelPositions:
+        """Returns the current distances measured by the drivetrain."""
+        positions = wpimath.MecanumDriveWheelPositions()
+        positions.frontLeft = self.frontLeftEncoder.getDistance()
+        positions.frontRight = self.frontRightEncoder.getDistance()
+        positions.rearLeft = self.backLeftEncoder.getDistance()
+        positions.rearRight = self.backRightEncoder.getDistance()
+        return positions
+
+    def setVelocities(self, velocities: wpimath.MecanumDriveWheelVelocities) -> None:
+        """Sets the desired velocities for each wheel."""
+        frontLeftFeedforward = self.feedforward.calculate(velocities.frontLeft)
+        frontRightFeedforward = self.feedforward.calculate(velocities.frontRight)
+        backLeftFeedforward = self.feedforward.calculate(velocities.rearLeft)
+        backRightFeedforward = self.feedforward.calculate(velocities.rearRight)
+
+        frontLeftOutput = self.frontLeftPIDController.calculate(
+            self.frontLeftEncoder.getRate(), velocities.frontLeft
+        )
+        frontRightOutput = self.frontRightPIDController.calculate(
+            self.frontRightEncoder.getRate(), velocities.frontRight
+        )
+        backLeftOutput = self.frontLeftPIDController.calculate(
+            self.backLeftEncoder.getRate(), velocities.rearLeft
+        )
+        backRightOutput = self.frontRightPIDController.calculate(
+            self.backRightEncoder.getRate(), velocities.rearRight
+        )
+
+        self.frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward)
+        self.frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward)
+        self.backLeftMotor.setVoltage(backLeftOutput + backLeftFeedforward)
+        self.backRightMotor.setVoltage(backRightOutput + backRightFeedforward)
+
+    def drive(
+        self,
+        xVelocity: float,
+        yVelocity: float,
+        rot: float,
+        fieldRelative: bool,
+        periodSeconds: float,
+    ) -> None:
+        """Method to drive the robot using joystick info."""
+        chassisVelocities = wpimath.ChassisVelocities(xVelocity, yVelocity, rot)
+        if fieldRelative:
+            chassisVelocities = chassisVelocities.toRobotRelative(
+                self.imu.getRotation2d()
+            )
+
+        self.setVelocities(
+            self.kinematics.toWheelVelocities(
+                chassisVelocities.discretize(periodSeconds)
+            ).desaturate(self.MAX_VELOCITY)
+        )
+
+    def updateOdometry(self) -> None:
+        """Updates the field-relative position."""
+        self.odometry.update(self.imu.getRotation2d(), self.getCurrentDistances())

robotpyExamples/examples/MecanumBot/robot.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpimath
+import wpilib
+
+from drivetrain import Drivetrain
+
+
+class MyRobot(wpilib.TimedRobot):
+    def __init__(self) -> None:
+        super().__init__()
+
+        self.controller = wpilib.NiDsXboxController(0)
+        self.mecanum = Drivetrain()
+
+        # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+        self.xvelocityLimiter = wpimath.SlewRateLimiter(3)
+        self.yvelocityLimiter = wpimath.SlewRateLimiter(3)
+        self.rotLimiter = wpimath.SlewRateLimiter(3)
+
+    def autonomousPeriodic(self) -> None:
+        self.driveWithJoystick(False)
+        self.mecanum.updateOdometry()
+
+    def teleopPeriodic(self) -> None:
+        self.driveWithJoystick(True)
+
+    def driveWithJoystick(self, fieldRelative: bool) -> None:
+        # Get the x velocity. We are inverting this because Xbox controllers return
+        # negative values when we push forward.
+        xVelocity = (
+            -self.xvelocityLimiter.calculate(self.controller.getLeftY())
+            * Drivetrain.MAX_VELOCITY
+        )
+
+        # Get the y velocity or sideways/strafe velocity. We are inverting this because
+        # we want a positive value when we pull to the left. Xbox controllers
+        # return positive values when you pull to the right by default.
+        yVelocity = (
+            -self.yvelocityLimiter.calculate(self.controller.getLeftX())
+            * Drivetrain.MAX_VELOCITY
+        )
+
+        # Get the rate of angular rotation. We are inverting this because we want a
+        # positive value when we pull to the left (remember, CCW is positive in
+        # mathematics). Xbox controllers return positive values when you pull to
+        # the right by default.
+        rot = (
+            -self.rotLimiter.calculate(self.controller.getRightX())
+            * Drivetrain.MAX_ANGULAR_VELOCITY
+        )
+
+        self.mecanum.drive(xVelocity, yVelocity, rot, fieldRelative, self.getPeriod())

robotpyExamples/examples/MecanumDrive/robot.py

@@ -0,0 +1,50 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+
+import wpilib
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    This is a sample program that uses mecanum drive with a gyro sensor to maintain rotation vectors
+    in relation to the starting orientation of the robot (field-oriented controls).
+    """
+
+    kFrontLeftChannel = 0
+    kRearLeftChannel = 1
+    kFrontRightChannel = 2
+    kRearRightChannel = 3
+    kIMUMountOrientation = wpilib.OnboardIMU.MountOrientation.FLAT
+    kJoystickPort = 0
+
+    def __init__(self) -> None:
+        """Robot initialization function"""
+        super().__init__()
+
+        self.imu = wpilib.OnboardIMU(self.kIMUMountOrientation)
+        self.joystick = wpilib.Joystick(self.kJoystickPort)
+
+        frontLeft = wpilib.PWMSparkMax(self.kFrontLeftChannel)
+        rearLeft = wpilib.PWMSparkMax(self.kRearLeftChannel)
+        frontRight = wpilib.PWMSparkMax(self.kFrontRightChannel)
+        rearRight = wpilib.PWMSparkMax(self.kRearRightChannel)
+
+        frontRight.setInverted(True)
+        rearRight.setInverted(True)
+
+        self.robotDrive = wpilib.MecanumDrive(
+            frontLeft, rearLeft, frontRight, rearRight
+        )
+
+    def teleopPeriodic(self) -> None:
+        self.robotDrive.driveCartesian(
+            -self.joystick.getY(),
+            -self.joystick.getX(),
+            -self.joystick.getZ(),
+            self.imu.getRotation2d(),
+        )

robotpyExamples/examples/MecanumDrivePoseEstimator/drivetrain.py

@@ -0,0 +1,163 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+
+import wpilib
+import wpimath
+import wpimath.units
+
+from exampleglobalmeasurementsensor import ExampleGlobalMeasurementSensor
+
+
+class Drivetrain:
+    """Represents a mecanum drive style drivetrain."""
+
+    MAX_VELOCITY = 3.0  # 3 meters per second
+    MAX_ANGULAR_VELOCITY = math.pi  # 1/2 rotation per second
+
+    def __init__(self) -> None:
+        self.frontLeftMotor = wpilib.PWMSparkMax(1)
+        self.frontRightMotor = wpilib.PWMSparkMax(2)
+        self.backLeftMotor = wpilib.PWMSparkMax(3)
+        self.backRightMotor = wpilib.PWMSparkMax(4)
+
+        self.frontLeftEncoder = wpilib.Encoder(0, 1)
+        self.frontRightEncoder = wpilib.Encoder(2, 3)
+        self.backLeftEncoder = wpilib.Encoder(4, 5)
+        self.backRightEncoder = wpilib.Encoder(6, 7)
+
+        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.frontLeftPIDController = wpimath.PIDController(1, 0, 0)
+        self.frontRightPIDController = wpimath.PIDController(1, 0, 0)
+        self.backLeftPIDController = wpimath.PIDController(1, 0, 0)
+        self.backRightPIDController = wpimath.PIDController(1, 0, 0)
+
+        self.imu = wpilib.OnboardIMU(wpilib.OnboardIMU.MountOrientation.FLAT)
+
+        self.kinematics = wpimath.MecanumDriveKinematics(
+            self.frontLeftLocation,
+            self.frontRightLocation,
+            self.backLeftLocation,
+            self.backRightLocation,
+        )
+
+        # Here we use MecanumDrivePoseEstimator so that we can fuse odometry readings. The numbers
+        # used below are robot specific, and should be tuned.
+        self.poseEstimator = wpimath.MecanumDrivePoseEstimator(
+            self.kinematics,
+            self.imu.getRotation2d(),
+            self.getCurrentDistances(),
+            wpimath.Pose2d(),
+            (0.05, 0.05, wpimath.units.degreesToRadians(5)),
+            (0.5, 0.5, wpimath.units.degreesToRadians(30)),
+        )
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.feedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3)
+
+        self.imu.resetYaw()
+
+        # We need to invert one side of the drivetrain so that positive voltages
+        # result in both sides moving forward. Depending on how your robot's
+        # gearbox is constructed, you might have to invert the left side instead.
+        self.frontRightMotor.setInverted(True)
+        self.backRightMotor.setInverted(True)
+
+    def getCurrentState(self) -> wpimath.MecanumDriveWheelVelocities:
+        """Returns the current state of the drivetrain.
+
+        :returns: The current state of the drivetrain.
+        """
+        return wpimath.MecanumDriveWheelVelocities(
+            self.frontLeftEncoder.getRate(),
+            self.frontRightEncoder.getRate(),
+            self.backLeftEncoder.getRate(),
+            self.backRightEncoder.getRate(),
+        )
+
+    def getCurrentDistances(self) -> wpimath.MecanumDriveWheelPositions:
+        """Returns the current distances measured by the drivetrain.
+
+        :returns: The current distances measured by the drivetrain.
+        """
+        positions = wpimath.MecanumDriveWheelPositions()
+        positions.frontLeft = self.frontLeftEncoder.getDistance()
+        positions.frontRight = self.frontRightEncoder.getDistance()
+        positions.rearLeft = self.backLeftEncoder.getDistance()
+        positions.rearRight = self.backRightEncoder.getDistance()
+        return positions
+
+    def setVelocities(self, velocities: wpimath.MecanumDriveWheelVelocities) -> None:
+        """Set the desired velocities for each wheel.
+
+        :param velocities: The desired wheel velocities.
+        """
+        frontLeftFeedforward = self.feedforward.calculate(velocities.frontLeft)
+        frontRightFeedforward = self.feedforward.calculate(velocities.frontRight)
+        backLeftFeedforward = self.feedforward.calculate(velocities.rearLeft)
+        backRightFeedforward = self.feedforward.calculate(velocities.rearRight)
+
+        frontLeftOutput = self.frontLeftPIDController.calculate(
+            self.frontLeftEncoder.getRate(), velocities.frontLeft
+        )
+        frontRightOutput = self.frontRightPIDController.calculate(
+            self.frontRightEncoder.getRate(), velocities.frontRight
+        )
+        backLeftOutput = self.backLeftPIDController.calculate(
+            self.backLeftEncoder.getRate(), velocities.rearLeft
+        )
+        backRightOutput = self.backRightPIDController.calculate(
+            self.backRightEncoder.getRate(), velocities.rearRight
+        )
+
+        self.frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward)
+        self.frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward)
+        self.backLeftMotor.setVoltage(backLeftOutput + backLeftFeedforward)
+        self.backRightMotor.setVoltage(backRightOutput + backRightFeedforward)
+
+    def drive(
+        self,
+        xVelocity: float,
+        yVelocity: float,
+        rot: float,
+        fieldRelative: bool,
+        period: float,
+    ) -> None:
+        """Method to drive the robot using joystick info.
+
+        :param xVelocity: Velocity of the robot in the x direction (forward).
+        :param yVelocity: Velocity of the robot in the y direction (sideways).
+        :param rot: Angular rate of the robot.
+        :param fieldRelative: Whether the provided x and y velocities are relative to the field.
+        """
+        chassisVelocities = wpimath.ChassisVelocities(xVelocity, yVelocity, rot)
+        if fieldRelative:
+            chassisVelocities = chassisVelocities.toRobotRelative(
+                self.poseEstimator.getEstimatedPosition().rotation()
+            )
+        self.setVelocities(
+            self.kinematics.toWheelVelocities(
+                chassisVelocities.discretize(period)
+            ).desaturate(self.MAX_VELOCITY)
+        )
+
+    def updateOdometry(self) -> None:
+        """Updates the field relative position of the robot."""
+        self.poseEstimator.update(self.imu.getRotation2d(), self.getCurrentDistances())
+
+        # Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
+        # a real robot, this must be calculated based either on latency or timestamps.
+        self.poseEstimator.addVisionMeasurement(
+            ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
+                self.poseEstimator.getEstimatedPosition()
+            ),
+            wpilib.Timer.getTimestamp() - 0.3,
+        )

robotpyExamples/examples/MecanumDrivePoseEstimator/exampleglobalmeasurementsensor.py

@@ -0,0 +1,34 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import random
+
+import wpimath
+import wpimath.units
+
+
+class ExampleGlobalMeasurementSensor:
+    """This dummy class represents a global measurement sensor, such as a computer vision
+    solution.
+    """
+
+    def __init__(self) -> None:
+        raise RuntimeError("Utility class")
+
+    @staticmethod
+    def getEstimatedGlobalPose(estimatedRobotPose: wpimath.Pose2d) -> wpimath.Pose2d:
+        """Get a "noisy" fake global pose reading.
+
+        :param estimatedRobotPose: The robot pose.
+        """
+        rand_x = random.gauss(0.0, 0.5)
+        rand_y = random.gauss(0.0, 0.5)
+        rand_rot = random.gauss(0.0, wpimath.units.degreesToRadians(30))
+        return wpimath.Pose2d(
+            estimatedRobotPose.x + rand_x,
+            estimatedRobotPose.y + rand_y,
+            estimatedRobotPose.rotation().rotateBy(wpimath.Rotation2d(rand_rot)),
+        )

robotpyExamples/examples/MecanumDrivePoseEstimator/robot.py

@@ -0,0 +1,52 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import wpimath
+
+from drivetrain import Drivetrain
+
+
+class MyRobot(wpilib.TimedRobot):
+    def __init__(self) -> None:
+        super().__init__()
+
+        self.controller = wpilib.NiDsXboxController(0)
+        self.mecanum = Drivetrain()
+
+        # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+        self.xvelocityLimiter = wpimath.SlewRateLimiter(3)
+        self.yvelocityLimiter = wpimath.SlewRateLimiter(3)
+        self.rotLimiter = wpimath.SlewRateLimiter(3)
+
+    def autonomousPeriodic(self) -> None:
+        self.driveWithJoystick(False)
+        self.mecanum.updateOdometry()
+
+    def teleopPeriodic(self) -> None:
+        self.driveWithJoystick(True)
+
+    def driveWithJoystick(self, fieldRelative: bool) -> None:
+        # Get the x velocity. We are inverting this because Xbox controllers return
+        # negative values when we push forward.
+        xVelocity = -self.xvelocityLimiter.calculate(self.controller.getLeftY())
+        xVelocity *= Drivetrain.MAX_VELOCITY
+
+        # Get the y velocity or sideways/strafe velocity. We are inverting this because
+        # we want a positive value when we pull to the left. Xbox controllers
+        # return positive values when you pull to the right by default.
+        yVelocity = -self.yvelocityLimiter.calculate(self.controller.getLeftX())
+        yVelocity *= Drivetrain.MAX_VELOCITY
+
+        # Get the rate of angular rotation. We are inverting this because we want a
+        # positive value when we pull to the left (remember, CCW is positive in
+        # mathematics). Xbox controllers return positive values when you pull to
+        # the right by default.
+        rot = -self.rotLimiter.calculate(self.controller.getRightX())
+        rot *= Drivetrain.MAX_ANGULAR_VELOCITY
+
+        self.mecanum.drive(xVelocity, yVelocity, rot, fieldRelative, self.getPeriod())

robotpyExamples/examples/Mechanism2d/robot.py

@@ -0,0 +1,60 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import wpiutil
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    This sample program shows how to use Mechanism2d - a visual representation of arms, elevators,
+    and other mechanisms on dashboards; driven by a node-based API.
+
+    Ligaments are based on other ligaments or roots, and roots are contained in the base
+    Mechanism2d object.
+    """
+
+    kMetersPerPulse = 0.01
+    kElevatorMinimumLength = 0.5
+
+    def __init__(self):
+        super().__init__()
+        self.elevatorMotor = wpilib.PWMSparkMax(0)
+        self.wristMotor = wpilib.PWMSparkMax(1)
+        self.wristPot = wpilib.AnalogPotentiometer(1, 90)
+        self.elevatorEncoder = wpilib.Encoder(0, 1)
+        self.joystick = wpilib.Joystick(0)
+
+        self.elevatorEncoder.setDistancePerPulse(self.kMetersPerPulse)
+
+        # the main mechanism object
+        self.mech = wpilib.Mechanism2d(3, 3)
+        # the mechanism root node
+        self.root = self.mech.getRoot("climber", 2, 0)
+
+        # MechanismLigament2d objects represent each "section"/"stage" of the mechanism, and are based
+        # off the root node or another ligament object
+        self.elevator = self.root.appendLigament(
+            "elevator", self.kElevatorMinimumLength, 90
+        )
+        self.wrist = self.elevator.appendLigament(
+            "wrist", 0.5, 90, 6, wpiutil.Color8Bit(wpiutil.Color.PURPLE)
+        )
+
+        # post the mechanism to the dashboard
+        wpilib.SmartDashboard.putData("Mech2d", self.mech)
+
+    def robotPeriodic(self):
+        # update the dashboard mechanism's state
+        self.elevator.setLength(
+            self.kElevatorMinimumLength + self.elevatorEncoder.getDistance()
+        )
+        self.wrist.setAngle(self.wristPot.get())
+
+    def teleopPeriodic(self):
+        self.elevatorMotor.setThrottle(self.joystick.getRawAxis(0))
+        self.wristMotor.setThrottle(self.joystick.getRawAxis(1))

robotpyExamples/examples/RapidReactCommandBot/constants.py

@@ -0,0 +1,89 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+
+import wpimath.units
+
+
+class DriveConstants:
+    kLeftMotor1Port = 0
+    kLeftMotor2Port = 1
+    kRightMotor1Port = 2
+    kRightMotor2Port = 3
+
+    kLeftEncoderPorts = (0, 1)
+    kRightEncoderPorts = (2, 3)
+    kLeftEncoderReversed = False
+    kRightEncoderReversed = True
+
+    kEncoderCPR = 1024
+    kWheelDiameter = wpimath.units.inchesToMeters(6)
+    kEncoderDistancePerPulse = (kWheelDiameter * math.pi) / kEncoderCPR
+
+    # These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+    # These values MUST be determined either experimentally or theoretically for *your* robot's
+    # drive. The SysId tool provides a convenient method for obtaining feedback and feedforward
+    # values for your robot.
+    kTurnP = 1.0
+    kTurnI = 0.0
+    kTurnD = 0.0
+
+    kTurnToleranceDeg = 5.0
+    kTurnRateToleranceDegPerS = 10.0  # degrees per second
+
+    kMaxTurnRateDegPerS = 100
+    kMaxTurnAccelerationDegPerSSquared = 300
+
+    kS = 1.0  # V
+    kV = 0.8  # V/(deg/s)
+    kA = 0.15  # V/(deg/s^2)
+
+
+class ShooterConstants:
+    kEncoderPorts = (4, 5)
+    kEncoderReversed = False
+    kEncoderCPR = 1024
+    # Distance units will be rotations
+    kEncoderDistancePerPulse = 1.0 / kEncoderCPR
+
+    kShooterMotorPort = 4
+    kFeederMotorPort = 5
+
+    kShooterFreeRPS = 5300.0
+    kShooterTargetRPS = 4000.0
+    kShooterToleranceRPS = 50.0
+
+    # These are not real PID gains, and will have to be tuned for your specific robot.
+    kP = 1.0
+
+    # On a real robot the feedforward constants should be empirically determined; these are
+    # reasonable guesses.
+    kS = 0.05  # V
+    # Should have value 12V at free velocity
+    kV = 12.0 / kShooterFreeRPS  # V/(rot/s)
+
+    kFeederVelocity = 0.5
+
+
+class IntakeConstants:
+    kMotorPort = 6
+    kSolenoidPorts = (2, 3)
+
+
+class StorageConstants:
+    kMotorPort = 7
+    kBallSensorPort = 6
+
+
+class AutoConstants:
+    kTimeout = 3
+    kDriveDistance = 2.0  # m
+    kDriveVelocity = 0.5
+
+
+class OIConstants:
+    kDriverControllerPort = 0

robotpyExamples/examples/RapidReactCommandBot/rapidreactcommandbot.py

@@ -0,0 +1,88 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+from commands2.button import CommandNiDsXboxController, Trigger
+
+from constants import AutoConstants, OIConstants, ShooterConstants
+from subsystems.drive import Drive
+from subsystems.intake import Intake
+from subsystems.pneumatics import Pneumatics
+from subsystems.shooter import Shooter
+from subsystems.storage import Storage
+
+
+class RapidReactCommandBot:
+    """This class is where the bulk of the robot should be declared. Since Command-based is a
+    "declarative" paradigm, very little robot logic should actually be handled in the
+    :class:`.Robot` periodic methods (other than the scheduler calls). Instead, the structure of
+    the robot (including subsystems, commands, and button mappings) should be declared here.
+    """
+
+    def __init__(self) -> None:
+        # The robot's subsystems
+        self.drive = Drive()
+        self.intake = Intake()
+        self.storage = Storage()
+        self.shooter = Shooter()
+        self.pneumatics = Pneumatics()
+
+        # The driver's controller
+        self.driverController = CommandNiDsXboxController(
+            OIConstants.kDriverControllerPort
+        )
+
+    def configureBindings(self) -> None:
+        """Use this method to define bindings between conditions and commands. These are useful for
+        automating robot behaviors based on button and sensor input.
+
+        Should be called in the robot class constructor.
+
+        Event binding methods are available on the :class:`.Trigger` class.
+        """
+        # Automatically run the storage motor whenever the ball storage is not full,
+        # and turn it off whenever it fills. Uses subsystem-hosted trigger to
+        # improve readability and make inter-subsystem communication easier.
+        self.storage.hasCargo.whileFalse(self.storage.runCommand())
+
+        # Automatically disable and retract the intake whenever the ball storage is full.
+        self.storage.hasCargo.onTrue(self.intake.retractCommand())
+
+        # Control the drive with split-stick arcade controls
+        self.drive.setDefaultCommand(
+            self.drive.arcadeDriveCommand(
+                lambda: -self.driverController.getLeftY(),
+                lambda: -self.driverController.getRightX(),
+            )
+        )
+
+        # Deploy the intake with the X button
+        self.driverController.x().onTrue(self.intake.intakeCommand())
+        # Retract the intake with the Y button
+        self.driverController.y().onTrue(self.intake.retractCommand())
+
+        # Fire the shooter with the A button
+        self.driverController.a().onTrue(
+            commands2.cmd.parallel(
+                self.shooter.shootCommand(ShooterConstants.kShooterTargetRPS),
+                self.storage.runCommand(),
+            ).withName("Shoot")
+        )
+
+        # Toggle compressor with the Start button
+        self.driverController.start().toggleOnTrue(
+            self.pneumatics.disableCompressorCommand()
+        )
+
+    def getAutonomousCommand(self) -> commands2.Command:
+        """Use this to define the command that runs during autonomous.
+
+        Scheduled during :meth:`.Robot.autonomousInit`.
+        """
+        # Drive forward for 2 meters at half velocity with a 3 second timeout
+        return self.drive.driveDistanceCommand(
+            AutoConstants.kDriveDistance, AutoConstants.kDriveVelocity
+        ).withTimeout(AutoConstants.kTimeout)

robotpyExamples/examples/RapidReactCommandBot/robot.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+import wpilib
+
+from rapidreactcommandbot import RapidReactCommandBot
+
+
+class MyRobot(commands2.TimedCommandRobot):
+    """The methods in this class are called automatically corresponding to each mode, as
+    described in the TimedRobot documentation. If you change the name of this class or the
+    package after creating this project, you must also update the Main.java file in the
+    project.
+    """
+
+    def __init__(self) -> None:
+        """This function is run when the robot is first started up and should be used for any
+        initialization code.
+        """
+        super().__init__()
+        self.autonomousCommand = None
+        self.robot = RapidReactCommandBot()
+
+        # Configure default commands and condition bindings on robot startup
+        self.robot.configureBindings()
+
+        # Initialize data logging.
+        wpilib.DataLogManager.start()
+
+    def disabledInit(self) -> None:
+        """This function is called once each time the robot enters Disabled mode."""
+        pass
+
+    def disabledPeriodic(self) -> None:
+        pass
+
+    def autonomousInit(self) -> None:
+        self.autonomousCommand = self.robot.getAutonomousCommand()
+
+        if self.autonomousCommand is not None:
+            self.autonomousCommand.schedule()
+
+    def autonomousPeriodic(self) -> None:
+        """This function is called periodically during autonomous."""
+        pass
+
+    def teleopInit(self) -> None:
+        # This makes sure that the autonomous stops running when
+        # teleop starts running. If you want the autonomous to
+        # continue until interrupted by another command, remove
+        # this line or comment it out.
+        if self.autonomousCommand is not None:
+            self.autonomousCommand.cancel()
+
+    def teleopPeriodic(self) -> None:
+        """This function is called periodically during operator control."""
+        pass
+
+    def utilityInit(self) -> None:
+        # Cancels all running commands at the start of utility mode.
+        commands2.CommandScheduler.getInstance().cancelAll()
+
+    def utilityPeriodic(self) -> None:
+        """This function is called periodically during utility mode."""
+        pass

robotpyExamples/examples/RapidReactCommandBot/subsystems/drive.py

@@ -0,0 +1,145 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from typing import Callable
+
+from commands2 import Command, Subsystem
+import wpilib
+import wpimath
+
+from constants import DriveConstants
+
+
+class Drive(Subsystem):
+    def __init__(self) -> None:
+        """Creates a new Drive subsystem."""
+        super().__init__()
+
+        # The motors on the left side of the drive.
+        self.leftLeader = wpilib.PWMSparkMax(DriveConstants.kLeftMotor1Port)
+        self.leftFollower = wpilib.PWMSparkMax(DriveConstants.kLeftMotor2Port)
+
+        # The motors on the right side of the drive.
+        self.rightLeader = wpilib.PWMSparkMax(DriveConstants.kRightMotor1Port)
+        self.rightFollower = wpilib.PWMSparkMax(DriveConstants.kRightMotor2Port)
+
+        # The robot's drive
+        self.drive = wpilib.DifferentialDrive(self.leftLeader, self.rightLeader)
+
+        # The left-side drive encoder
+        self.leftEncoder = wpilib.Encoder(
+            DriveConstants.kLeftEncoderPorts[0],
+            DriveConstants.kLeftEncoderPorts[1],
+            DriveConstants.kLeftEncoderReversed,
+        )
+
+        # The right-side drive encoder
+        self.rightEncoder = wpilib.Encoder(
+            DriveConstants.kRightEncoderPorts[0],
+            DriveConstants.kRightEncoderPorts[1],
+            DriveConstants.kRightEncoderReversed,
+        )
+
+        self.imu = wpilib.OnboardIMU(wpilib.OnboardIMU.MountOrientation.FLAT)
+        self.controller = wpimath.ProfiledPIDController(
+            DriveConstants.kTurnP,
+            DriveConstants.kTurnI,
+            DriveConstants.kTurnD,
+            wpimath.TrapezoidProfile.Constraints(
+                DriveConstants.kMaxTurnRateDegPerS,
+                DriveConstants.kMaxTurnAccelerationDegPerSSquared,
+            ),
+        )
+        self.feedforward = wpimath.SimpleMotorFeedforwardMeters(
+            DriveConstants.kS, DriveConstants.kV, DriveConstants.kA
+        )
+
+        self.leftLeader.addFollower(self.leftFollower)
+        self.rightLeader.addFollower(self.rightFollower)
+
+        # We need to invert one side of the drivetrain so that positive voltages
+        # result in both sides moving forward. Depending on how your robot's
+        # gearbox is constructed, you might have to invert the left side instead.
+        self.rightLeader.setInverted(True)
+
+        # Sets the distance per pulse for the encoders
+        self.leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse)
+        self.rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse)
+
+        # Set the controller to be continuous (because it is an angle controller)
+        self.controller.enableContinuousInput(-180, 180)
+        # Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
+        # setpoint before it is considered as having reached the reference
+        self.controller.setTolerance(
+            DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS
+        )
+
+    def arcadeDriveCommand(
+        self, fwd: Callable[[], float], rot: Callable[[], float]
+    ) -> Command:
+        """Returns a command that drives the robot with arcade controls.
+
+        :param fwd: the commanded forward movement
+        :param rot: the commanded rotation
+        """
+        # A split-stick arcade command, with forward/backward controlled by the left
+        # hand, and turning controlled by the right.
+        return self.run(lambda: self.drive.arcadeDrive(fwd(), rot())).withName(
+            "arcadeDrive"
+        )
+
+    def driveDistanceCommand(self, distance: float, velocity: float) -> Command:
+        """Returns a command that drives the robot forward a specified distance at a specified
+        velocity.
+
+        :param distance: The distance to drive forward in meters
+        :param velocity: The fraction of max velocity at which to drive
+        """
+        return (
+            self.runOnce(
+                lambda: (
+                    self.leftEncoder.reset(),
+                    self.rightEncoder.reset(),
+                )
+            )
+            .andThen(self.run(lambda: self.drive.arcadeDrive(velocity, 0)))
+            .until(
+                lambda: max(
+                    self.leftEncoder.getDistance(), self.rightEncoder.getDistance()
+                )
+                >= distance
+            )
+            .finallyDo(lambda interrupted: self.drive.stopMotor())
+        )
+
+    def turnToAngleCommand(self, angleDeg: float) -> Command:
+        """Returns a command that turns to robot to the specified angle using a motion profile and
+        PID controller.
+
+        :param angleDeg: The angle to turn to
+        """
+
+        def _reset() -> None:
+            self.controller.reset(self.imu.getRotation2d().degrees())
+
+        def _run() -> None:
+            rotation_output = self.controller.calculate(
+                self.imu.getRotation2d().degrees(), angleDeg
+            )
+            feedforward = self.feedforward.calculate(
+                self.controller.getSetpoint().velocity
+            )
+            self.drive.arcadeDrive(
+                0,
+                rotation_output
+                + feedforward / wpilib.RobotController.getBatteryVoltage(),
+            )
+
+        return (
+            self.startRun(_reset, _run)
+            .until(self.controller.atGoal)
+            .finallyDo(lambda interrupted: self.drive.arcadeDrive(0, 0))
+        )

robotpyExamples/examples/RapidReactCommandBot/subsystems/intake.py

@@ -0,0 +1,43 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from commands2 import Command, Subsystem
+import wpilib
+
+from constants import IntakeConstants
+
+
+class Intake(Subsystem):
+    def __init__(self) -> None:
+        super().__init__()
+        self.motor = wpilib.PWMSparkMax(IntakeConstants.kMotorPort)
+
+        # Double solenoid connected to two channels of a PCM with the default CAN ID
+        self.pistons = wpilib.DoubleSolenoid(
+            0,
+            wpilib.PneumaticsModuleType.CTRE_PCM,
+            IntakeConstants.kSolenoidPorts[0],
+            IntakeConstants.kSolenoidPorts[1],
+        )
+
+    def intakeCommand(self) -> Command:
+        """Returns a command that deploys the intake, and then runs the intake motor
+        indefinitely.
+        """
+        return (
+            self.runOnce(lambda: self.pistons.set(wpilib.DoubleSolenoid.Value.FORWARD))
+            .andThen(self.run(lambda: self.motor.set(1.0)))
+            .withName("Intake")
+        )
+
+    def retractCommand(self) -> Command:
+        """Returns a command that turns off and retracts the intake."""
+        return self.runOnce(
+            lambda: (
+                self.motor.disable(),
+                self.pistons.set(wpilib.DoubleSolenoid.Value.REVERSE),
+            )
+        ).withName("Retract")

robotpyExamples/examples/RapidReactCommandBot/subsystems/pneumatics.py

@@ -0,0 +1,56 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from commands2 import Command, Subsystem
+import wpilib
+
+
+class Pneumatics(Subsystem):
+    """Subsystem for managing the compressor, pressure sensor, etc."""
+
+    # External analog pressure sensor
+    # product-specific voltage->pressure conversion, see product manual
+    # in this case, 250(V/5)-25
+    # the scale parameter in the AnalogPotentiometer constructor is scaled from 1 instead of 5,
+    # so if r is the raw AnalogPotentiometer output, the pressure is 250r-25
+    kScale = 250.0
+    kOffset = -25.0
+
+    def __init__(self) -> None:
+        super().__init__()
+        self.pressureTransducer = wpilib.AnalogPotentiometer(
+            # the AnalogIn port
+            2,
+            self.kScale,
+            self.kOffset,
+        )
+
+        # Compressor connected to a PCM with a default CAN ID (0)
+        self.compressor = wpilib.Compressor(0, wpilib.PneumaticsModuleType.CTRE_PCM)
+
+    def getPressure(self) -> float:
+        """Query the analog pressure sensor.
+
+        :returns: the measured pressure, in PSI
+        """
+        # Get the pressure (in PSI) from an analog pressure sensor connected to the RIO.
+        return self.pressureTransducer.get()
+
+    def disableCompressorCommand(self) -> Command:
+        """Disable the compressor closed-loop for as long as the command runs.
+
+        Structured this way as the compressor is enabled by default.
+
+        :returns: command
+        """
+        return self.startEnd(
+            # Disable closed-loop mode on the compressor.
+            self.compressor.disable,
+            # Enable closed-loop mode based on the digital pressure switch connected to the
+            # PCM/PH.
+            # The switch is open when the pressure is over ~120 PSI.
+            self.compressor.enableDigital,
+        ).withName("Compressor Disabled")

robotpyExamples/examples/RapidReactCommandBot/subsystems/shooter.py

@@ -0,0 +1,69 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from commands2 import Command, InstantCommand, Subsystem, cmd
+import wpilib
+import wpimath
+
+from constants import ShooterConstants
+
+
+class Shooter(Subsystem):
+    def __init__(self) -> None:
+        """The shooter subsystem for the robot."""
+        super().__init__()
+        self.shooterMotor = wpilib.PWMSparkMax(ShooterConstants.kShooterMotorPort)
+        self.feederMotor = wpilib.PWMSparkMax(ShooterConstants.kFeederMotorPort)
+        self.shooterEncoder = wpilib.Encoder(
+            ShooterConstants.kEncoderPorts[0],
+            ShooterConstants.kEncoderPorts[1],
+            ShooterConstants.kEncoderReversed,
+        )
+        self.shooterFeedforward = wpimath.SimpleMotorFeedforwardMeters(
+            ShooterConstants.kS, ShooterConstants.kV
+        )
+        self.shooterFeedback = wpimath.PIDController(ShooterConstants.kP, 0.0, 0.0)
+
+        self.shooterFeedback.setTolerance(ShooterConstants.kShooterToleranceRPS)
+        self.shooterEncoder.setDistancePerPulse(
+            ShooterConstants.kEncoderDistancePerPulse
+        )
+
+        # Set default command to turn off both the shooter and feeder motors, and then idle
+        self.setDefaultCommand(
+            self.runOnce(
+                lambda: (
+                    self.shooterMotor.disable(),
+                    self.feederMotor.disable(),
+                )
+            )
+            .andThen(self.run(lambda: None))
+            .withName("Idle")
+        )
+
+    def shootCommand(self, setpointRotationsPerSecond: float) -> Command:
+        """Returns a command to shoot the balls currently stored in the robot. Spins the shooter
+        flywheel up to the specified setpoint, and then runs the feeder motor.
+
+        :param setpointRotationsPerSecond: The desired shooter velocity
+        """
+
+        def _run_shooter() -> None:
+            self.shooterMotor.set(
+                self.shooterFeedforward.calculate(setpointRotationsPerSecond)
+                + self.shooterFeedback.calculate(
+                    self.shooterEncoder.getRate(), setpointRotationsPerSecond
+                )
+            )
+
+        return cmd.parallel(
+            # Run the shooter flywheel at the desired setpoint using feedforward and feedback
+            self.run(_run_shooter),
+            # Wait until the shooter has reached the setpoint, and then run the feeder
+            cmd.waitUntil(self.shooterFeedback.atSetpoint).andThen(
+                InstantCommand(lambda: self.feederMotor.set(1))
+            ),
+        ).withName("Shoot")

robotpyExamples/examples/RapidReactCommandBot/subsystems/storage.py

@@ -0,0 +1,35 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from commands2 import Command, Subsystem
+from commands2.button import Trigger
+import wpilib
+
+from constants import StorageConstants
+
+
+class Storage(Subsystem):
+    def __init__(self) -> None:
+        """Create a new Storage subsystem."""
+        super().__init__()
+        self.motor = wpilib.PWMSparkMax(StorageConstants.kMotorPort)
+        self.ballSensor = wpilib.DigitalInput(StorageConstants.kBallSensorPort)
+
+        # Expose trigger from subsystem to improve readability and ease
+        # inter-subsystem communications
+        # Whether the ball storage is full.
+        self.hasCargo = Trigger(self.ballSensor.get)
+
+        # Set default command to turn off the storage motor and then idle
+        self.setDefaultCommand(
+            self.runOnce(self.motor.disable)
+            .andThen(self.run(lambda: None))
+            .withName("Idle")
+        )
+
+    def runCommand(self) -> Command:
+        """Returns a command that runs the storage motor indefinitely."""
+        return self.run(lambda: self.motor.set(1)).withName("run")

robotpyExamples/examples/RomiReference/commands/arcadedrive.py

@@ -0,0 +1,34 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import typing
+import commands2
+from subsystems.drivetrain import Drivetrain
+
+
+class ArcadeDrive(commands2.Command):
+    def __init__(
+        self,
+        drive: Drivetrain,
+        forward: typing.Callable[[], float],
+        rotation: typing.Callable[[], float],
+    ) -> None:
+        """Creates a new ArcadeDrive. This command will drive your robot according to the velocity supplier
+        lambdas. This command does not terminate.
+
+        :param drivetrain:  The drivetrain subsystem on which this command will run
+        :param forward:     Callable supplier of forward/backward velocity
+        :param rotation:    Callable supplier of rotational velocity
+        """
+
+        self.drive = drive
+        self.forward = forward
+        self.rotation = rotation
+
+        self.addRequirements(self.drive)
+
+    def execute(self) -> None:
+        self.drive.arcadeDrive(self.forward(), self.rotation())

robotpyExamples/examples/RomiReference/commands/autonomousdistance.py

@@ -0,0 +1,28 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+
+from commands.drivedistance import DriveDistance
+from commands.turndegrees import TurnDegrees
+from subsystems.drivetrain import Drivetrain
+
+
+class AutonomousDistance(commands2.SequentialCommandGroup):
+    def __init__(self, drive: Drivetrain) -> None:
+        """Creates a new Autonomous Drive based on distance. This will drive out for a specified distance,
+        turn around and drive back.
+
+        :param drivetrain: The drivetrain subsystem on which this command will run
+        """
+        super().__init__()
+
+        self.addCommands(
+            DriveDistance(-0.5, 10, drive),
+            TurnDegrees(-0.5, 180, drive),
+            DriveDistance(-0.5, 10, drive),
+            TurnDegrees(0.5, 180, drive),
+        )

robotpyExamples/examples/RomiReference/commands/autonomoustime.py

@@ -0,0 +1,30 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+
+from commands.drivetime import DriveTime
+from commands.turntime import TurnTime
+from subsystems.drivetrain import Drivetrain
+
+
+class AutonomousTime(commands2.SequentialCommandGroup):
+    def __init__(self, drive: Drivetrain) -> None:
+        """Creates a new Autonomous Drive based on time. This will drive out for a period of time, turn
+        around for time (equivalent to time to turn around) and drive forward again. This should mimic
+        driving out, turning around and driving back.
+
+        :param drivetrain: The drive subsystem on which this command will run
+
+        """
+        super().__init__()
+
+        self.addCommands(
+            DriveTime(-0.6, 2.0, drive),
+            TurnTime(-0.5, 1.3, drive),
+            DriveTime(-0.6, 2.0, drive),
+            TurnTime(0.5, 1.3, drive),
+        )

robotpyExamples/examples/RomiReference/commands/drivedistance.py

@@ -0,0 +1,43 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+
+from subsystems.drivetrain import Drivetrain
+
+
+class DriveDistance(commands2.Command):
+    def __init__(self, velocity: float, inches: float, drive: Drivetrain) -> None:
+        """Creates a new DriveDistance. This command will drive your your robot for a desired distance at
+        a desired velocity.
+
+        :param velocity:  The velocity at which the robot will drive
+        :param inches: The number of inches the robot will drive
+        :param drive:  The drivetrain subsystem on which this command will run
+        """
+
+        self.distance = inches
+        self.velocity = velocity
+        self.drive = drive
+        self.addRequirements(drive)
+
+    def initialize(self) -> None:
+        """Called when the command is initially scheduled."""
+        self.drive.arcadeDrive(0, 0)
+        self.drive.resetEncoders()
+
+    def execute(self) -> None:
+        """Called every time the scheduler runs while the command is scheduled."""
+        self.drive.arcadeDrive(self.velocity, 0)
+
+    def end(self, interrupted: bool) -> None:
+        """Called once the command ends or is interrupted."""
+        self.drive.arcadeDrive(0, 0)
+
+    def isFinished(self) -> bool:
+        """Returns true when the command should end."""
+        # Compare distance travelled from start to desired distance
+        return abs(self.drive.getAverageDistanceInch()) >= self.distance

robotpyExamples/examples/RomiReference/commands/drivetime.py

@@ -0,0 +1,46 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+import wpilib
+
+from subsystems.drivetrain import Drivetrain
+
+
+class DriveTime(commands2.Command):
+    """Creates a new DriveTime. This command will drive your robot for a desired velocity and time."""
+
+    def __init__(self, velocity: float, time: float, drive: Drivetrain) -> None:
+        """Creates a new DriveTime. This command will drive your robot for a desired velocity and time.
+
+        :param velocity: The velocity which the robot will drive. Negative is in reverse.
+        :param time:  How much time to drive in seconds
+        :param drive: The drivetrain subsystem on which this command will run
+        """
+
+        self.velocity = velocity
+        self.duration = time
+        self.drive = drive
+        self.addRequirements(drive)
+
+        self.startTime = 0.0
+
+    def initialize(self) -> None:
+        """Called when the command is initially scheduled."""
+        self.startTime = wpilib.Timer.getTimestamp()
+        self.drive.arcadeDrive(0, 0)
+
+    def execute(self) -> None:
+        """Called every time the scheduler runs while the command is scheduled."""
+        self.drive.arcadeDrive(self.velocity, 0)
+
+    def end(self, interrupted: bool) -> None:
+        """Called once the command ends or is interrupted."""
+        self.drive.arcadeDrive(0, 0)
+
+    def isFinished(self) -> bool:
+        """Returns true when the command should end"""
+        return wpilib.Timer.getTimestamp() - self.startTime >= self.duration

robotpyExamples/examples/RomiReference/commands/turndegrees.py

@@ -0,0 +1,57 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+import commands2
+
+from subsystems.drivetrain import Drivetrain
+
+
+class TurnDegrees(commands2.Command):
+    def __init__(self, velocity: float, degrees: float, drive: Drivetrain) -> None:
+        """Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in
+        degrees) and rotational velocity.
+
+        :param velocity:   The velocity which the robot will drive. Negative is in reverse.
+        :param degrees: Degrees to turn. Leverages encoders to compare distance.
+        :param drive:   The drive subsystem on which this command will run
+        """
+
+        self.degrees = degrees
+        self.velocity = velocity
+        self.drive = drive
+        self.addRequirements(drive)
+
+    def initialize(self) -> None:
+        """Called when the command is initially scheduled."""
+        # Set motors to stop, read encoder values for starting point
+        self.drive.arcadeDrive(0, 0)
+        self.drive.resetEncoders()
+
+    def execute(self) -> None:
+        """Called every time the scheduler runs while the command is scheduled."""
+        self.drive.arcadeDrive(0, self.velocity)
+
+    def end(self, interrupted: bool) -> None:
+        """Called once the command ends or is interrupted."""
+        self.drive.arcadeDrive(0, 0)
+
+    def isFinished(self) -> bool:
+        """Returns true when the command should end."""
+
+        # Need to convert distance travelled to degrees. The Standard
+        # Romi Chassis found here, https://www.pololu.com/category/203/romi-chassis-kits,
+        # has a wheel placement diameter (149 mm) - width of the wheel (8 mm) = 141 mm
+        # or 5.551 inches. We then take into consideration the width of the tires.
+        inchPerDegree = math.pi * 5.551 / 360.0
+
+        # Compare distance travelled from start to distance based on degree turn
+        return self._getAverageTurningDistance() >= inchPerDegree * self.degrees
+
+    def _getAverageTurningDistance(self) -> float:
+        leftDistance = abs(self.drive.getLeftDistanceInch())
+        rightDistance = abs(self.drive.getRightDistanceInch())
+        return (leftDistance + rightDistance) / 2.0

robotpyExamples/examples/RomiReference/commands/turntime.py

@@ -0,0 +1,48 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+import wpilib
+
+from subsystems.drivetrain import Drivetrain
+
+
+class TurnTime(commands2.Command):
+    """Creates a new TurnTime command. This command will turn your robot for a
+    desired rotational velocity and time.
+    """
+
+    def __init__(self, velocity: float, time: float, drive: Drivetrain) -> None:
+        """Creates a new TurnTime.
+
+        :param velocity: The velocity which the robot will turn. Negative is in reverse.
+        :param time:  How much time to turn in seconds
+        :param drive: The drive subsystem on which this command will run
+        """
+
+        self.rotationalVelocity = velocity
+        self.duration = time
+        self.drive = drive
+        self.addRequirements(drive)
+
+        self.startTime = 0.0
+
+    def initialize(self) -> None:
+        """Called when the command is initially scheduled."""
+        self.startTime = wpilib.Timer.getTimestamp()
+        self.drive.arcadeDrive(0, 0)
+
+    def execute(self) -> None:
+        """Called every time the scheduler runs while the command is scheduled."""
+        self.drive.arcadeDrive(0, self.rotationalVelocity)
+
+    def end(self, interrupted: bool) -> None:
+        """Called once the command ends or is interrupted."""
+        self.drive.arcadeDrive(0, 0)
+
+    def isFinished(self) -> bool:
+        """Returns true when the command should end"""
+        return wpilib.Timer.getTimestamp() - self.startTime >= self.duration

robotpyExamples/examples/RomiReference/robot.py

@@ -0,0 +1,97 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+#
+# Example that shows how to connect to a ROMI from RobotPy
+#
+# Requirements
+# ------------
+#
+# You must have the robotpy-halsim-ws package installed. This is best done via:
+#
+#    # Windows
+#    py -3 -m pip install robotpy[commands2,sim]
+#
+#    # Linux/macOS
+#    pip3 install robotpy[commands2,sim]
+#
+# Run the program
+# ---------------
+#
+# Use the dedicated ROMI command:
+#
+#    # Windows
+#    py -3 -m robotpy run-romi
+#
+#    # Linux/macOS
+#    python -m robotpy run-romi
+#
+# If your ROMI isn't at the default address, use --host/--port:
+#
+#    python -m robotpy run-romi --host 10.0.0.2 --port 3300
+#
+# By default the WPILib simulation GUI will be displayed. To disable the display
+# you can add the --nogui option.
+#
+
+import typing
+
+import wpilib
+import commands2
+
+from robotcontainer import RobotContainer
+
+
+class MyRobot(commands2.TimedCommandRobot):
+    """
+    Command v2 robots are encouraged to inherit from TimedCommandRobot, which
+    has an implementation of robotPeriodic which runs the scheduler for you
+    """
+
+    autonomousCommand: typing.Optional[commands2.Command] = None
+
+    def __init__(self) -> None:
+        """
+        This function is run when the robot is first started up and should be used for any
+        initialization code.
+        """
+
+        super().__init__()
+        # Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+        # autonomous chooser on the dashboard.
+        self.container = RobotContainer()
+
+    def disabledInit(self) -> None:
+        """This function is called once each time the robot enters Disabled mode."""
+
+    def disabledPeriodic(self) -> None:
+        """This function is called periodically when disabled"""
+
+    def autonomousInit(self) -> None:
+        """This autonomous runs the autonomous command selected by your RobotContainer class."""
+        self.autonomousCommand = self.container.getAutonomousCommand()
+
+        if self.autonomousCommand:
+            self.autonomousCommand.schedule()
+
+    def autonomousPeriodic(self) -> None:
+        """This function is called periodically during autonomous"""
+
+    def teleopInit(self) -> None:
+        # This makes sure that the autonomous stops running when
+        # teleop starts running. If you want the autonomous to
+        # continue until interrupted by another command, remove
+        # this line or comment it out.
+        if self.autonomousCommand:
+            self.autonomousCommand.cancel()
+
+    def teleopPeriodic(self) -> None:
+        """This function is called periodically during operator control"""
+
+    def utilityInit(self) -> None:
+        # Cancels all running commands at the start of utility mode
+        commands2.CommandScheduler.getInstance().cancelAll()

robotpyExamples/examples/RomiReference/robotcontainer.py

@@ -0,0 +1,90 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import typing
+
+import commands2
+import commands2.button
+import wpilib
+import romi
+
+from commands.arcadedrive import ArcadeDrive
+from commands.autonomousdistance import AutonomousDistance
+from commands.autonomoustime import AutonomousTime
+
+from subsystems.drivetrain import Drivetrain
+
+
+class RobotContainer:
+    """
+    This class is where the bulk of the robot should be declared. Since Command-based is a
+    "declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot`
+    periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+    subsystems, commands, and button mappings) should be declared here.
+    """
+
+    def __init__(self) -> None:
+        # The robot's subsystems and commands are defined here...
+        self.drivetrain = Drivetrain()
+        self.onboardIO = romi.OnBoardIO(
+            romi.OnBoardIO.ChannelMode.INPUT, romi.OnBoardIO.ChannelMode.INPUT
+        )
+
+        # Assumes a gamepad plugged into channnel 0
+        self.controller = wpilib.Joystick(0)
+
+        # Create SmartDashboard chooser for autonomous routines
+        self.chooser = wpilib.SendableChooser()
+
+        # NOTE: The I/O pin functionality of the 5 exposed I/O pins depends on the hardware "overlay"
+        # that is specified when launching the wpilib-ws server on the Romi raspberry pi.
+        # By default, the following are available (listed in order from inside of the board to outside):
+        # - DIO 8 (mapped to Arduino pin 11, closest to the inside of the board)
+        # - Analog In 0 (mapped to Analog Channel 6 / Arduino Pin 4)
+        # - Analog In 1 (mapped to Analog Channel 2 / Arduino Pin 20)
+        # - PWM 2 (mapped to Arduino Pin 21)
+        # - PWM 3 (mapped to Arduino Pin 22)
+        #
+        # Your subsystem configuration should take the overlays into account
+
+        self._configureButtonBindings()
+
+    def _configureButtonBindings(self):
+        """Use this method to define your button->command mappings. Buttons can be created by
+        instantiating a :class:`.GenericHID` or one of its subclasses (:class`.Joystick or
+        :class:`.XboxController`), and then passing it to a :class:`.JoystickButton`.
+        """
+
+        # Default command is arcade drive. This will run unless another command
+        # is scheduler over it
+        self.drivetrain.setDefaultCommand(self.getArcadeDriveCommand())
+
+        # Example of how to use the onboard IO
+        onboardButtonA = commands2.button.Trigger(self.onboardIO.getButtonAPressed)
+        onboardButtonA.onTrue(commands2.PrintCommand("Button A Pressed")).onFalse(
+            commands2.PrintCommand("Button A Released")
+        )
+
+        # Setup SmartDashboard options
+        self.chooser.setDefaultOption(
+            "Auto Routine Distance", AutonomousDistance(self.drivetrain)
+        )
+        self.chooser.addOption("Auto Routine Time", AutonomousTime(self.drivetrain))
+        wpilib.SmartDashboard.putData(self.chooser)
+
+    def getAutonomousCommand(self) -> typing.Optional[commands2.Command]:
+        return self.chooser.getSelected()
+
+    def getArcadeDriveCommand(self) -> ArcadeDrive:
+        """Use this to pass the teleop command to the main robot class.
+
+        :returns: the command to run in teleop
+        """
+        return ArcadeDrive(
+            self.drivetrain,
+            lambda: -self.controller.getRawAxis(1),
+            lambda: -self.controller.getRawAxis(2),
+        )

robotpyExamples/examples/RomiReference/subsystems/drivetrain.py

@@ -0,0 +1,103 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+
+import commands2
+import wpilib
+import romi
+
+
+class Drivetrain(commands2.Subsystem):
+    kCountsPerRevolution = 1440.0
+    kWheelDiameterInch = 2.75591
+
+    def __init__(self) -> None:
+
+        # The Romi has the left and right motors set to
+        # PWM channels 0 and 1 respectively
+        self.leftMotor = wpilib.Spark(0)
+        self.rightMotor = wpilib.Spark(1)
+
+        # We need to invert one side of the drivetrain so that positive voltages
+        # result in both sides moving forward. Depending on how your robot's
+        # gearbox is constructed, you might have to invert the left side instead.
+        self.rightMotor.setInverted(True)
+
+        # The Romi has onboard encoders that are hardcoded
+        # to use DIO pins 4/5 and 6/7 for the left and right
+        self.leftEncoder = wpilib.Encoder(4, 5)
+        self.rightEncoder = wpilib.Encoder(6, 7)
+
+        # Set up the differential drive controller
+        self.drive = wpilib.DifferentialDrive(self.leftMotor, self.rightMotor)
+
+        # Set up the RomiGyro
+        self.gyro = romi.RomiGyro()
+
+        # Use inches as unit for encoder distances
+        self.leftEncoder.setDistancePerPulse(
+            (math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution
+        )
+        self.rightEncoder.setDistancePerPulse(
+            (math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution
+        )
+        self.resetEncoders()
+
+    def arcadeDrive(self, fwd: float, rot: float) -> None:
+        """
+        Drives the robot using arcade controls.
+
+        :param fwd: the commanded forward movement
+        :param rot: the commanded rotation
+        """
+        self.drive.arcadeDrive(fwd, rot)
+
+    def resetEncoders(self) -> None:
+        """Resets the drive encoders to currently read a position of 0."""
+        self.leftEncoder.reset()
+        self.rightEncoder.reset()
+
+    def getLeftEncoderCount(self) -> int:
+        return self.leftEncoder.get()
+
+    def getRightEncoderCount(self) -> int:
+        return self.rightEncoder.get()
+
+    def getLeftDistanceInch(self) -> float:
+        return self.leftEncoder.getDistance()
+
+    def getRightDistanceInch(self) -> float:
+        return self.rightEncoder.getDistance()
+
+    def getAverageDistanceInch(self) -> float:
+        """Gets the average distance of the TWO encoders."""
+        return (self.getLeftDistanceInch() + self.getRightDistanceInch()) / 2.0
+
+    def getGyroAngleX(self) -> float:
+        """Current angle of the Romi around the X-axis.
+
+        :returns: The current angle of the Romi in degrees
+        """
+        return self.gyro.getAngleX()
+
+    def getGyroAngleY(self) -> float:
+        """Current angle of the Romi around the Y-axis.
+
+        :returns: The current angle of the Romi in degrees
+        """
+        return self.gyro.getAngleY()
+
+    def getGyroAngleZ(self) -> float:
+        """Current angle of the Romi around the Z-axis.
+
+        :returns: The current angle of the Romi in degrees
+        """
+        return self.gyro.getAngleZ()
+
+    def resetGyro(self) -> None:
+        """Reset the gyro"""
+        self.gyro.reset()

robotpyExamples/examples/SimpleDifferentialDriveSimulation/drivetrain.py

@@ -0,0 +1,162 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+
+import wpilib
+import wpilib.simulation
+import wpimath
+
+
+class Drivetrain:
+    """Represents a differential drive style drivetrain."""
+
+    # 3 meters per second.
+    kMaxVelocity = 3.0
+    # 1/2 rotation per second.
+    kMaxAngularVelocity = math.pi
+
+    kTrackwidth = 0.381 * 2
+    kWheelRadius = 0.0508
+    kEncoderResolution = -4096
+
+    def __init__(self) -> None:
+        self.leftLeader = wpilib.PWMSparkMax(1)
+        self.leftFollower = wpilib.PWMSparkMax(2)
+        self.rightLeader = wpilib.PWMSparkMax(3)
+        self.rightFollower = wpilib.PWMSparkMax(4)
+
+        self.leftEncoder = wpilib.Encoder(0, 1)
+        self.rightEncoder = wpilib.Encoder(2, 3)
+
+        self.leftPIDController = wpimath.PIDController(8.5, 0, 0)
+        self.rightPIDController = wpimath.PIDController(8.5, 0, 0)
+
+        self.imu = wpilib.OnboardIMU(wpilib.OnboardIMU.MountOrientation.FLAT)
+
+        self.kinematics = wpimath.DifferentialDriveKinematics(self.kTrackwidth)
+        self.odometry = wpimath.DifferentialDriveOdometry(
+            self.imu.getRotation2d(),
+            self.leftEncoder.getDistance(),
+            self.rightEncoder.getDistance(),
+        )
+
+        # Gains are for example purposes only - must be determined for your own
+        # robot!
+        self.feedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3)
+
+        # Simulation classes help us simulate our robot
+        self.leftEncoderSim = wpilib.simulation.EncoderSim(self.leftEncoder)
+        self.rightEncoderSim = wpilib.simulation.EncoderSim(self.rightEncoder)
+        self.fieldSim = wpilib.Field2d()
+        self.drivetrainSystem = wpimath.Models.differentialDriveFromSysId(
+            1.98, 0.2, 1.5, 0.3
+        )
+        self.drivetrainSimulator = wpilib.simulation.DifferentialDrivetrainSim(
+            self.drivetrainSystem,
+            self.kTrackwidth,
+            wpimath.DCMotor.CIM(2),
+            8,
+            self.kWheelRadius,
+        )
+
+        # Subsystem constructor.
+        self.leftLeader.addFollower(self.leftFollower)
+        self.rightLeader.addFollower(self.rightFollower)
+
+        # We need to invert one side of the drivetrain so that positive voltages
+        # result in both sides moving forward. Depending on how your robot's
+        # gearbox is constructed, you might have to invert the left side instead.
+        self.rightLeader.setInverted(True)
+
+        # Set the distance per pulse for the drive encoders. We can simply use the
+        # distance traveled for one rotation of the wheel divided by the encoder
+        # resolution.
+        self.leftEncoder.setDistancePerPulse(
+            2 * math.pi * self.kWheelRadius / self.kEncoderResolution
+        )
+        self.rightEncoder.setDistancePerPulse(
+            2 * math.pi * self.kWheelRadius / self.kEncoderResolution
+        )
+
+        self.leftEncoder.reset()
+        self.rightEncoder.reset()
+
+        self.rightLeader.setInverted(True)
+        wpilib.SmartDashboard.putData("Field", self.fieldSim)
+
+    def setVelocities(
+        self, velocities: wpimath.DifferentialDriveWheelVelocities
+    ) -> None:
+        """Sets velocities to the drivetrain motors."""
+        leftFeedforward = self.feedforward.calculate(velocities.left)
+        rightFeedforward = self.feedforward.calculate(velocities.right)
+        leftOutput = self.leftPIDController.calculate(
+            self.leftEncoder.getRate(), velocities.left
+        )
+        rightOutput = self.rightPIDController.calculate(
+            self.rightEncoder.getRate(), velocities.right
+        )
+
+        self.leftLeader.setVoltage(leftOutput + leftFeedforward)
+        self.rightLeader.setVoltage(rightOutput + rightFeedforward)
+
+    def drive(self, xVelocity: float, rot: float) -> None:
+        """Controls the robot using arcade drive.
+
+        :param xVelocity: the velocity for the x axis
+        :param rot: the rotation
+        """
+        self.setVelocities(
+            self.kinematics.toWheelVelocities(
+                wpimath.ChassisVelocities(xVelocity, 0, rot)
+            )
+        )
+
+    def updateOdometry(self) -> None:
+        """Update robot odometry."""
+        self.odometry.update(
+            self.imu.getRotation2d(),
+            self.leftEncoder.getDistance(),
+            self.rightEncoder.getDistance(),
+        )
+
+    def resetOdometry(self, pose: wpimath.Pose2d) -> None:
+        """Resets robot odometry."""
+        self.drivetrainSimulator.setPose(pose)
+        self.odometry.resetPosition(
+            self.imu.getRotation2d(),
+            self.leftEncoder.getDistance(),
+            self.rightEncoder.getDistance(),
+            pose,
+        )
+
+    def getPose(self) -> wpimath.Pose2d:
+        """Check the current robot pose."""
+        return self.odometry.getPose()
+
+    def simulationPeriodic(self) -> None:
+        """Update our simulation. This should be run every robot loop in simulation."""
+        # To update our simulation, we set motor voltage inputs, update the
+        # simulation, and write the simulated positions and velocities to our
+        # simulated encoder and gyro. We negate the right side so that positive
+        # voltages make the right side move forward.
+        self.drivetrainSimulator.setInputs(
+            self.leftLeader.getThrottle() * wpilib.RobotController.getInputVoltage(),
+            self.rightLeader.getThrottle() * wpilib.RobotController.getInputVoltage(),
+        )
+        self.drivetrainSimulator.update(0.02)
+
+        self.leftEncoderSim.setDistance(self.drivetrainSimulator.getLeftPosition())
+        self.leftEncoderSim.setRate(self.drivetrainSimulator.getLeftVelocity())
+        self.rightEncoderSim.setDistance(self.drivetrainSimulator.getRightPosition())
+        self.rightEncoderSim.setRate(self.drivetrainSimulator.getRightVelocity())
+        # self.gyroSim.setAngle(-self.drivetrainSimulator.getHeading().getDegrees())
+
+    def periodic(self) -> None:
+        """Update odometry - this should be run every robot loop."""
+        self.updateOdometry()
+        self.fieldSim.setRobotPose(self.odometry.getPose())

robotpyExamples/examples/SimpleDifferentialDriveSimulation/robot.py

@@ -0,0 +1,68 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import wpimath
+
+from drivetrain import Drivetrain
+
+
+class MyRobot(wpilib.TimedRobot):
+    def __init__(self) -> None:
+        super().__init__()
+        self.controller = wpilib.NiDsXboxController(0)
+
+        # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
+        # to 1.
+        self.velocityLimiter = wpimath.SlewRateLimiter(3)
+        self.rotLimiter = wpimath.SlewRateLimiter(3)
+
+        self.drive = Drivetrain()
+        self.feedback = wpimath.LTVUnicycleController(0.020)
+        self.timer = wpilib.Timer()
+
+        # Called once at the beginning of the robot program.
+        self.trajectory = wpimath.TrajectoryGenerator.generateTrajectory(
+            wpimath.Pose2d(2, 2, wpimath.Rotation2d()),
+            [],
+            wpimath.Pose2d(6, 4, wpimath.Rotation2d()),
+            wpimath.TrajectoryConfig(2, 2),
+        )
+
+    def robotPeriodic(self) -> None:
+        self.drive.periodic()
+
+    def autonomousInit(self) -> None:
+        self.timer.restart()
+        self.drive.resetOdometry(self.trajectory.initialPose())
+
+    def autonomousPeriodic(self) -> None:
+        elapsed = self.timer.get()
+        reference = self.trajectory.sample(elapsed)
+        velocities = self.feedback.calculate(self.drive.getPose(), reference)
+        self.drive.drive(velocities.vx, velocities.omega)
+
+    def teleopPeriodic(self) -> None:
+        # Get the x velocity. We are inverting this because Xbox controllers return
+        # negative values when we push forward.
+        xVelocity = (
+            -self.velocityLimiter.calculate(self.controller.getLeftY())
+            * Drivetrain.kMaxVelocity
+        )
+
+        # Get the rate of angular rotation. We are inverting this because we want a
+        # positive value when we pull to the left (remember, CCW is positive in
+        # mathematics). Xbox controllers return positive values when you pull to
+        # the right by default.
+        rot = (
+            -self.rotLimiter.calculate(self.controller.getRightX())
+            * Drivetrain.kMaxAngularVelocity
+        )
+        self.drive.drive(xVelocity, rot)
+
+    def simulationPeriodic(self) -> None:
+        self.drive.simulationPeriodic()

robotpyExamples/examples/StateSpaceArm/robot.py

@@ -0,0 +1,146 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+import wpilib
+import wpimath.units
+import wpimath
+
+kMotorPort = 0
+kEncoderAChannel = 0
+kEncoderBChannel = 1
+kJoystickPort = 0
+kRaisedPosition = wpimath.units.degreesToRadians(90.0)
+kLoweredPosition = wpimath.units.degreesToRadians(0.0)
+
+# Moment of inertia of the arm, in kg * m^2. Can be estimated with CAD. If finding this constant
+# is difficult, LinearSystem.identifyPositionSystem may be better.
+kArmMOI = 1.2
+
+# Reduction between motors and encoder, as output over input. If the arm spins slower than
+# the motors, this number should be greater than one.
+kArmGearing = 10.0
+
+
+class MyRobot(wpilib.TimedRobot):
+    """This is a sample program to demonstrate how to use a state-space controller to control an arm."""
+
+    def __init__(self) -> None:
+        super().__init__()
+
+        self.profile = wpimath.TrapezoidProfile(
+            wpimath.TrapezoidProfile.Constraints(
+                wpimath.units.degreesToRadians(45),
+                wpimath.units.degreesToRadians(
+                    90
+                ),  # Max arm velocity and acceleration.
+            )
+        )
+
+        self.lastProfiledReference = wpimath.TrapezoidProfile.State()
+
+        # The plant holds a state-space model of our arm. This system has the following properties:
+        #
+        # States: [position, velocity], in radians and radians per second.
+        # Inputs (what we can "put in"): [voltage], in volts.
+        # Outputs (what we can measure): [position], in radians.
+        self.armPlant = wpimath.Models.singleJointedArmFromPhysicalConstants(
+            wpimath.DCMotor.NEO(2),
+            kArmMOI,
+            kArmGearing,
+        ).slice(0)
+
+        # The observer fuses our encoder data and voltage inputs to reject noise.
+        self.observer = wpimath.KalmanFilter_2_1_1(
+            self.armPlant,
+            # How accurate we think our model is, in radians and radians/sec.
+            (
+                0.015,
+                0.17,
+            ),
+            # How accurate we think our encoder position data is. In this case we very highly trust our encoder position reading.
+            (0.01,),
+            0.020,
+        )
+
+        # A LQR uses feedback to create voltage commands.
+        self.controller = wpimath.LinearQuadraticRegulator_2_1(
+            self.armPlant,
+            # qelms. Velocity error tolerance, in radians and radians per second.
+            # Decrease this to more heavily penalize state excursion, or make the
+            # controller behave more aggressively.
+            (
+                wpimath.units.degreesToRadians(1.0),
+                wpimath.units.degreesToRadians(10.0),
+            ),
+            # relms. Control effort (voltage) tolerance. Decrease this to more
+            # heavily penalize control effort, or make the controller less
+            # aggressive. 12 is a good starting point because that is the
+            # (approximate) maximum voltage of a battery.
+            (12.0,),
+            # Nominal time between loops. 20ms for TimedRobot, but can be lower if
+            # using notifiers.
+            0.020,
+        )
+
+        # The state-space loop combines a controller, observer, feedforward and plant for easy control.
+        self.loop = wpimath.LinearSystemLoop_2_1_1(
+            self.armPlant, self.controller, self.observer, 12.0, 0.020
+        )
+
+        # An encoder set up to measure flywheel velocity in radians per second.
+        self.encoder = wpilib.Encoder(kEncoderAChannel, kEncoderBChannel)
+
+        self.motor = wpilib.PWMSparkMax(kMotorPort)
+
+        # A joystick to read the trigger from.
+        self.joystick = wpilib.Joystick(kJoystickPort)
+
+        # We go 2 pi radians in 1 rotation, or 4096 counts.
+        self.encoder.setDistancePerPulse(math.tau / 4096)
+
+    def teleopInit(self) -> None:
+        # Reset our loop to make sure it's in a known state.
+        self.loop.reset([self.encoder.getDistance(), self.encoder.getRate()])
+
+        # Reset our last reference to the current state.
+        self.lastProfiledReference = wpimath.TrapezoidProfile.State(
+            self.encoder.getDistance(), self.encoder.getRate()
+        )
+
+    def teleopPeriodic(self) -> None:
+        # Sets the target position of our arm. This is similar to setting the setpoint of a
+        # PID controller.
+
+        if self.joystick.getTrigger():
+            # the trigger is pressed, so we go to the high goal.
+            goal = wpimath.TrapezoidProfile.State(kRaisedPosition, 0.0)
+
+        else:
+            # Otherwise, we go to the low goal
+            goal = wpimath.TrapezoidProfile.State(kLoweredPosition, 0.0)
+
+        # Step our TrapezoidalProfile forward 20ms and set it as our next reference
+        self.lastProfiledReference = self.profile.calculate(
+            0.020, self.lastProfiledReference, goal
+        )
+        self.loop.setNextR(
+            [self.lastProfiledReference.position, self.lastProfiledReference.velocity]
+        )
+
+        # Correct our Kalman filter's state vector estimate with encoder data.
+        self.loop.correct([self.encoder.getDistance()])
+
+        # Update our LQR to generate new voltage commands and use the voltages to predict the next
+        # state with out Kalman filter.
+        self.loop.predict(0.020)
+
+        # Send the new calculated voltage to the motors.
+        # voltage = duty cycle * battery voltage, so
+        # duty cycle = voltage / battery voltage
+        nextVoltage = self.loop.U(0)
+        self.motor.setVoltage(nextVoltage)

robotpyExamples/examples/StateSpaceElevator/robot.py

@@ -0,0 +1,153 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+import wpilib
+import wpimath
+import wpimath.units
+
+kMotorPort = 0
+kEncoderAChannel = 0
+kEncoderBChannel = 1
+kJoystickPort = 0
+kHighGoalPosition = wpimath.units.feetToMeters(3)
+kLowGoalPosition = wpimath.units.feetToMeters(0)
+
+kCarriageMass = 4.5
+# kilograms
+
+# A 1.5in diameter drum has a radius of 0.75in, or 0.019in.
+kDrumRadius = 1.5 / 2.0 * 25.4 / 1000.0
+
+# Reduction between motors and encoder, as output over input. If the elevator spins slower than
+# the motors, this number should be greater than one.
+kElevatorGearing = 6.0
+
+
+class MyRobot(wpilib.TimedRobot):
+    """This is a sample program to demonstrate how to use a state-space controller to control an
+    elevator.
+    """
+
+    def __init__(self) -> None:
+        super().__init__()
+
+        self.profile = wpimath.TrapezoidProfile(
+            wpimath.TrapezoidProfile.Constraints(
+                wpimath.units.feetToMeters(3.0),
+                wpimath.units.feetToMeters(
+                    6.0
+                ),  # Max elevator velocity and acceleration.
+            )
+        )
+
+        self.lastProfiledReference = wpimath.TrapezoidProfile.State()
+
+        # The plant holds a state-space model of our elevator. This system has the following properties:
+
+        # States: [position, velocity], in meters and meters per second.
+        # Inputs (what we can "put in"): [voltage], in volts.
+        # Outputs (what we can measure): [position], in meters.
+
+        # This elevator is driven by two NEO motors.
+        self.elevatorPlant = wpimath.Models.elevatorFromPhysicalConstants(
+            wpimath.DCMotor.NEO(2),
+            kCarriageMass,
+            kDrumRadius,
+            kElevatorGearing,
+        ).slice(0)
+
+        # The observer fuses our encoder data and voltage inputs to reject noise.
+        self.observer = wpimath.KalmanFilter_2_1_1(
+            self.elevatorPlant,
+            (
+                wpimath.units.inchesToMeters(2),
+                wpimath.units.inchesToMeters(40),
+            ),  # How accurate we think our model is, in meters and meters/second.
+            (
+                0.001,
+            ),  # How accurate we think our encoder position data is. In this case we very highly trust our encoder position reading.
+            0.020,
+        )
+
+        # A LQR uses feedback to create voltage commands.
+        self.controller = wpimath.LinearQuadraticRegulator_2_1(
+            self.elevatorPlant,
+            # qelms. State error tolerance, in meters and meters per second.
+            # Decrease this to more heavily penalize state excursion, or make the
+            # controller behave more aggressively.
+            (
+                wpimath.units.inchesToMeters(1.0),
+                wpimath.units.inchesToMeters(10.0),
+            ),
+            # relms. Control effort (voltage) tolerance. Decrease this to more
+            # heavily penalize control effort, or make the controller less
+            # aggressive. 12 is a good starting point because that is the
+            # (approximate) maximum voltage of a battery.
+            (12.0,),
+            # Nominal time between loops. 20ms for TimedRobot, but can be lower if
+            # using notifiers.
+            0.020,
+        )
+
+        # The state-space loop combines a controller, observer, feedforward and plant for easy control.
+        self.loop = wpimath.LinearSystemLoop_2_1_1(
+            self.elevatorPlant, self.controller, self.observer, 12.0, 0.020
+        )
+
+        # An encoder set up to measure elevator height in meters.
+        self.encoder = wpilib.Encoder(kEncoderAChannel, kEncoderBChannel)
+
+        self.motor = wpilib.PWMSparkMax(kMotorPort)
+
+        # A joystick to read the trigger from.
+        self.joystick = wpilib.Joystick(kJoystickPort)
+
+        # Circumference = pi * d, so distance per click = pi * d / counts
+        self.encoder.setDistancePerPulse(math.tau * kDrumRadius / 4096)
+
+    def teleopInit(self) -> None:
+        # Reset our loop to make sure it's in a known state.
+        self.loop.reset([self.encoder.getDistance(), self.encoder.getRate()])
+
+        # Reset our last reference to the current state.
+        self.lastProfiledReference = wpimath.TrapezoidProfile.State(
+            self.encoder.getDistance(), self.encoder.getRate()
+        )
+
+    def teleopPeriodic(self) -> None:
+        # Sets the target position of our arm. This is similar to setting the setpoint of a
+        # PID controller.
+
+        if self.joystick.getTrigger():
+            # the trigger is pressed, so we go to the high goal.
+            goal = wpimath.TrapezoidProfile.State(kHighGoalPosition, 0.0)
+
+        else:
+            # Otherwise, we go to the low goal
+            goal = wpimath.TrapezoidProfile.State(kLowGoalPosition, 0.0)
+
+        # Step our TrapezoidalProfile forward 20ms and set it as our next reference
+        self.lastProfiledReference = self.profile.calculate(
+            0.020, self.lastProfiledReference, goal
+        )
+        self.loop.setNextR(
+            [self.lastProfiledReference.position, self.lastProfiledReference.velocity]
+        )
+
+        # Correct our Kalman filter's state vector estimate with encoder data.
+        self.loop.correct([self.encoder.getDistance()])
+
+        # Update our LQR to generate new voltage commands and use the voltages to predict the next
+        # state with out Kalman filter.
+        self.loop.predict(0.020)
+
+        # Send the new calculated voltage to the motors.
+        # voltage = duty cycle * battery voltage, so
+        # duty cycle = voltage / battery voltage
+        nextVoltage = self.loop.U(0)
+        self.motor.setVoltage(nextVoltage)

robotpyExamples/examples/StateSpaceFlywheel/robot.py

@@ -0,0 +1,113 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+
+import wpilib
+import wpimath.units
+
+kMotorPort = 0
+kEncoderAChannel = 0
+kEncoderBChannel = 1
+kJoystickPort = 0
+
+kSpinUpRadPerSec = 500.0
+kFlywheelMomentOfInertia = 0.00032  # kg/m^2
+
+# Reduction between motors and encoder, as output over input. If the flywheel spins slower than
+# the motors, this number should be greater than one.
+kFlywheelGearing = 1.0
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    This is a sample program to demonstrate how to use a state-space controller to control a
+    flywheel.
+    """
+
+    def __init__(self) -> None:
+        super().__init__()
+
+        self.kSpinUpRadPerSec = wpimath.units.rotationsPerMinuteToRadiansPerSecond(500)
+
+        # The plant holds a state-space model of our flywheel. This system has the following properties:
+        #
+        # States: [velocity], in radians per second.
+        # Inputs (what we can "put in"): [voltage], in volts.
+        # Outputs (what we can measure): [velocity], in radians per second.
+        self.flywheelPlant = wpimath.Models.flywheelFromPhysicalConstants(
+            wpimath.DCMotor.NEO(2),
+            kFlywheelMomentOfInertia,
+            kFlywheelGearing,
+        )
+
+        # The observer fuses our encoder data and voltage inputs to reject noise.
+        self.observer = wpimath.KalmanFilter_1_1_1(
+            self.flywheelPlant,
+            (3,),  # How accurate we think our model is
+            (0.01,),  # How accurate we think our encoder data is
+            0.020,
+        )
+
+        # A LQR uses feedback to create voltage commands.
+        self.controller = wpimath.LinearQuadraticRegulator_1_1(
+            self.flywheelPlant,
+            # qelms. Velocity error tolerance, in radians per second. Decrease
+            # this to more heavily penalize state excursion, or make the controller behave more
+            # aggressively.
+            (8,),
+            # relms. Control effort (voltage) tolerance. Decrease this to more
+            # heavily penalize control effort, or make the controller less aggressive. 12 is a good
+            # starting point because that is the (approximate) maximum voltage of a battery.
+            (12,),
+            # Nominal time between loops. 0.020 for TimedRobot, but can be lower if using notifiers.
+            0.020,
+        )
+
+        # The state-space loop combines a controller, observer, feedforward and plant for easy control.
+        self.loop = wpimath.LinearSystemLoop_1_1_1(
+            self.flywheelPlant, self.controller, self.observer, 12.0, 0.020
+        )
+
+        # An encoder set up to measure flywheel velocity in radians per second.
+        self.encoder = wpilib.Encoder(kEncoderAChannel, kEncoderBChannel)
+
+        self.motor = wpilib.PWMSparkMax(kMotorPort)
+
+        # A joystick to read the trigger from.
+        self.joystick = wpilib.Joystick(kJoystickPort)
+
+        # We go 2 pi radians per 4096 clicks.
+        self.encoder.setDistancePerPulse(math.tau / 4096)
+
+    def teleopInit(self) -> None:
+        self.loop.reset([self.encoder.getRate()])
+
+    def teleopPeriodic(self) -> None:
+
+        # Sets the target velocity of our flywheel. This is similar to setting the setpoint of a
+        # PID controller.
+        if self.joystick.getTriggerPressed():
+            # We just pressed the trigger, so let's set our next reference
+            self.loop.setNextR([kSpinUpRadPerSec])
+
+        elif self.joystick.getTriggerReleased():
+            # We just released the trigger, so let's spin down
+            self.loop.setNextR([0.0])
+
+        # Correct our Kalman filter's state vector estimate with encoder data.
+        self.loop.correct([self.encoder.getRate()])
+
+        # Update our LQR to generate new voltage commands and use the voltages to predict the next
+        # state with out Kalman filter.
+        self.loop.predict(0.020)
+
+        # Send the new calculated voltage to the motors.
+        # voltage = duty cycle * battery voltage, so
+        # duty cycle = voltage / battery voltage
+        nextVoltage = self.loop.U(0)
+        self.motor.setVoltage(nextVoltage)

robotpyExamples/examples/StateSpaceFlywheelSysId/robot.py

@@ -0,0 +1,101 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+import wpilib
+import wpimath
+import wpimath.units
+
+kMotorPort = 0
+kEncoderAChannel = 0
+kEncoderBChannel = 1
+kJoystickPort = 0
+kSpinupRadPerSec = wpimath.units.rotationsPerMinuteToRadiansPerSecond(500.0)
+
+# Volts per (radian per second)
+kFlywheelKv = 0.023
+
+# Volts per (radian per second squared)
+kFlywheelKa = 0.001
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    This is a sample program to demonstrate how to use a state-space controller to control a
+    flywheel.
+    """
+
+    def __init__(self) -> None:
+        super().__init__()
+
+        # The plant holds a state-space model of our flywheel. This system has the following properties:
+        #
+        # States: [velocity], in radians per second.
+        # Inputs (what we can "put in"): [voltage], in volts.
+        # Outputs (what we can measure): [velocity], in radians per second.
+        #
+        # The Kv and Ka constants are found using the SysID tool.
+        self.flywheelPlant = wpimath.Models.flywheelFromSysId(kFlywheelKv, kFlywheelKa)
+
+        # The observer fuses our encoder data and voltage inputs to reject noise.
+        self.observer = wpimath.KalmanFilter_1_1_1(
+            self.flywheelPlant,
+            (3,),  # How accurate we think our model is
+            (0.01,),  # How accurate we think our encoder data is
+            0.020,
+        )
+
+        # A LQR uses feedback to create voltage commands.
+        self.controller = wpimath.LinearQuadraticRegulator_1_1(
+            self.flywheelPlant,
+            (8,),  # Velocity error tolerance
+            (12,),  # Control effort (voltage) tolerance
+            0.020,
+        )
+
+        # The state-space loop combines a controller, observer, feedforward and plant for easy control.
+        self.loop = wpimath.LinearSystemLoop_1_1_1(
+            self.flywheelPlant, self.controller, self.observer, 12.0, 0.020
+        )
+
+        # An encoder set up to measure flywheel velocity in radians per second.
+        self.encoder = wpilib.Encoder(kEncoderAChannel, kEncoderBChannel)
+
+        self.motor = wpilib.PWMSparkMax(kMotorPort)
+
+        # A joystick to read the trigger from.
+        self.joystick = wpilib.Joystick(kJoystickPort)
+
+        # We go 2 pi radians per 4096 clicks.
+        self.encoder.setDistancePerPulse(math.tau / 4096)
+
+    def teleopInit(self) -> None:
+        self.loop.reset([self.encoder.getRate()])
+
+    def teleopPeriodic(self) -> None:
+        # Sets the target velocity of our flywheel. This is similar to setting the setpoint of a
+        # PID controller.
+        if self.joystick.getTriggerPressed():
+            # We just pressed the trigger, so let's set our next reference
+            self.loop.setNextR([kSpinupRadPerSec])
+
+        elif self.joystick.getTriggerReleased():
+            # We just released the trigger, so let's spin down
+            self.loop.setNextR([0])
+
+        # Correct our Kalman filter's state vector estimate with encoder data.
+        self.loop.correct([self.encoder.getRate()])
+
+        # Update our LQR to generate new voltage commands and use the voltages to predict the next
+        # state with out Kalman filter.
+        self.loop.predict(0.020)
+
+        # Send the new calculated voltage to the motors.
+        # voltage = duty cycle * battery voltage, so
+        # duty cycle = voltage / battery voltage
+        nextVoltage = self.loop.U(0)
+        self.motor.setVoltage(nextVoltage)

robotpyExamples/examples/SwerveBot/drivetrain.py

@@ -0,0 +1,98 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+import wpilib
+import swervemodule
+import wpimath
+
+kMaxVelocity = 3.0  # 3 meters per second
+kMaxAngularVelocity = math.pi  # 1/2 rotation per second
+
+
+class Drivetrain:
+    """
+    Represents a swerve drive style drivetrain.
+    """
+
+    def __init__(self) -> None:
+        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)
+        self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7)
+        self.backLeft = swervemodule.SwerveModule(5, 6, 8, 9, 10, 11)
+        self.backRight = swervemodule.SwerveModule(7, 8, 12, 13, 14, 15)
+
+        self.imu = wpilib.OnboardIMU(wpilib.OnboardIMU.MountOrientation.FLAT)
+
+        self.kinematics = wpimath.SwerveDrive4Kinematics(
+            self.frontLeftLocation,
+            self.frontRightLocation,
+            self.backLeftLocation,
+            self.backRightLocation,
+        )
+
+        self.odometry = wpimath.SwerveDrive4Odometry(
+            self.kinematics,
+            self.imu.getRotation2d(),
+            (
+                self.frontLeft.getPosition(),
+                self.frontRight.getPosition(),
+                self.backLeft.getPosition(),
+                self.backRight.getPosition(),
+            ),
+        )
+
+        self.imu.resetYaw()
+
+    def drive(
+        self,
+        xVelocity: float,
+        yVelocity: float,
+        rot: float,
+        fieldRelative: bool,
+        periodSeconds: float,
+    ) -> None:
+        """
+        Method to drive the robot using joystick info.
+        :param xVelocity: Velocity of the robot in the x direction (forward).
+        :param yVelocity: Velocity of the robot in the y direction (sideways).
+        :param rot: Angular rate of the robot.
+        :param fieldRelative: Whether the provided x and y velocities are relative to the field.
+        :param periodSeconds: Time
+        """
+        chassisVelocities = wpimath.ChassisVelocities(xVelocity, yVelocity, rot)
+        if fieldRelative:
+            chassisVelocities = chassisVelocities.toRobotRelative(
+                self.imu.getRotation2d()
+            )
+
+        chassisVelocities = chassisVelocities.discretize(periodSeconds)
+
+        velocities = wpimath.SwerveDrive4Kinematics.desaturateWheelVelocities(
+            self.kinematics.toSwerveModuleVelocities(chassisVelocities),
+            kMaxVelocity,
+        )
+
+        self.frontLeft.setDesiredVelocity(velocities[0])
+        self.frontRight.setDesiredVelocity(velocities[1])
+        self.backLeft.setDesiredVelocity(velocities[2])
+        self.backRight.setDesiredVelocity(velocities[3])
+
+    def updateOdometry(self) -> None:
+        """Updates the field relative position of the robot."""
+        self.odometry.update(
+            self.imu.getRotation2d(),
+            (
+                self.frontLeft.getPosition(),
+                self.frontRight.getPosition(),
+                self.backLeft.getPosition(),
+                self.backRight.getPosition(),
+            ),
+        )

robotpyExamples/examples/SwerveBot/robot.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import wpimath
+import drivetrain
+
+
+class MyRobot(wpilib.TimedRobot):
+    def __init__(self) -> None:
+        """Robot initialization function"""
+        super().__init__()
+        self.controller = wpilib.NiDsXboxController(0)
+        self.swerve = drivetrain.Drivetrain()
+
+        # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+        self.xvelocityLimiter = wpimath.SlewRateLimiter(3)
+        self.yvelocityLimiter = wpimath.SlewRateLimiter(3)
+        self.rotLimiter = wpimath.SlewRateLimiter(3)
+
+    def autonomousPeriodic(self) -> None:
+        self.driveWithJoystick(False)
+        self.swerve.updateOdometry()
+
+    def teleopPeriodic(self) -> None:
+        self.driveWithJoystick(True)
+
+    def driveWithJoystick(self, fieldRelative: bool) -> None:
+        # Get the x velocity. We are inverting this because Xbox controllers return
+        # negative values when we push forward.
+        xVelocity = (
+            -self.xvelocityLimiter.calculate(
+                wpimath.applyDeadband(self.controller.getLeftY(), 0.02)
+            )
+            * drivetrain.kMaxVelocity
+        )
+
+        # Get the y velocity or sideways/strafe velocity. We are inverting this because
+        # we want a positive value when we pull to the left. Xbox controllers
+        # return positive values when you pull to the right by default.
+        yVelocity = (
+            -self.yvelocityLimiter.calculate(
+                wpimath.applyDeadband(self.controller.getLeftX(), 0.02)
+            )
+            * drivetrain.kMaxVelocity
+        )
+
+        # Get the rate of angular rotation. We are inverting this because we want a
+        # positive value when we pull to the left (remember, CCW is positive in
+        # mathematics). Xbox controllers return positive values when you pull to
+        # the right by default.
+        rot = (
+            -self.rotLimiter.calculate(
+                wpimath.applyDeadband(self.controller.getRightX(), 0.02)
+            )
+            * drivetrain.kMaxAngularVelocity
+        )
+
+        self.swerve.drive(xVelocity, yVelocity, rot, fieldRelative, self.getPeriod())

robotpyExamples/examples/SwerveBot/swervemodule.py

@@ -0,0 +1,132 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+
+import wpilib
+import wpimath
+
+kWheelRadius = 0.0508
+kEncoderResolution = 4096
+kModuleMaxAngularVelocity = math.pi
+kModuleMaxAngularAcceleration = math.tau
+
+
+class SwerveModule:
+    def __init__(
+        self,
+        driveMotorChannel: int,
+        turningMotorChannel: int,
+        driveEncoderChannelA: int,
+        driveEncoderChannelB: int,
+        turningEncoderChannelA: int,
+        turningEncoderChannelB: int,
+    ) -> None:
+        """Constructs a SwerveModule with a drive motor, turning motor, drive encoder and turning encoder.
+
+        :param driveMotorChannel:      PWM output for the drive motor.
+        :param turningMotorChannel:    PWM output for the turning motor.
+        :param driveEncoderChannelA:   DIO input for the drive encoder channel A
+        :param driveEncoderChannelB:   DIO input for the drive encoder channel B
+        :param turningEncoderChannelA: DIO input for the turning encoder channel A
+        :param turningEncoderChannelB: DIO input for the turning encoder channel B
+        """
+        self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel)
+        self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel)
+
+        self.driveEncoder = wpilib.Encoder(driveEncoderChannelA, driveEncoderChannelB)
+        self.turningEncoder = wpilib.Encoder(
+            turningEncoderChannelA, turningEncoderChannelB
+        )
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.drivePIDController = wpimath.PIDController(1, 0, 0)
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.turningPIDController = wpimath.ProfiledPIDController(
+            1,
+            0,
+            0,
+            wpimath.TrapezoidProfile.Constraints(
+                kModuleMaxAngularVelocity,
+                kModuleMaxAngularAcceleration,
+            ),
+        )
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.driveFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3)
+        self.turnFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 0.5)
+
+        # 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
+        # resolution.
+        self.driveEncoder.setDistancePerPulse(
+            math.tau * kWheelRadius / kEncoderResolution
+        )
+
+        # Set the distance (in this case, angle) in radians per pulse for the turning encoder.
+        # This is the the angle through an entire rotation (2 * pi) divided by the
+        # encoder resolution.
+        self.turningEncoder.setDistancePerPulse(math.tau / kEncoderResolution)
+
+        # Limit the PID Controller's input range between -pi and pi and set the input
+        # to be continuous.
+        self.turningPIDController.enableContinuousInput(-math.pi, math.pi)
+
+    def getVelocity(self) -> wpimath.SwerveModuleVelocity:
+        """Returns the current velocity of the module.
+
+        :returns: The current velocity of the module.
+        """
+        return wpimath.SwerveModuleVelocity(
+            self.driveEncoder.getRate(),
+            wpimath.Rotation2d(self.turningEncoder.getDistance()),
+        )
+
+    def getPosition(self) -> wpimath.SwerveModulePosition:
+        """Returns the current position of the module.
+
+        :returns: The current position of the module.
+        """
+        return wpimath.SwerveModulePosition(
+            self.driveEncoder.getDistance(),
+            wpimath.Rotation2d(self.turningEncoder.getDistance()),
+        )
+
+    def setDesiredVelocity(self, desiredVelocity: wpimath.SwerveModuleVelocity) -> None:
+        """Sets the desired velocity for the module.
+
+        :param desiredVelocity: Desired state with velocity and angle.
+        """
+
+        encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance())
+
+        # Optimize the desired velocity to avoid spinning further than 90 degrees, then scale
+        # velocity 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.
+        velocity = desiredVelocity.optimize(encoderRotation).cosineScale(
+            encoderRotation
+        )
+
+        # Calculate the drive output from the drive PID controller.
+        driveOutput = self.drivePIDController.calculate(
+            self.driveEncoder.getRate(), velocity.velocity
+        )
+
+        driveFeedforward = self.driveFeedforward.calculate(velocity.velocity)
+
+        # Calculate the turning motor output from the turning PID controller.
+        turnOutput = self.turningPIDController.calculate(
+            self.turningEncoder.getDistance(), velocity.angle.radians()
+        )
+
+        turnFeedforward = self.turnFeedforward.calculate(
+            self.turningPIDController.getSetpoint().velocity
+        )
+
+        self.driveMotor.setVoltage(driveOutput + driveFeedforward)
+        self.turningMotor.setVoltage(turnOutput + turnFeedforward)

robotpyExamples/examples/SwerveDrivePoseEstimator/drivetrain.py

@@ -0,0 +1,113 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+
+import wpilib
+import wpimath
+import wpimath.units
+
+import swervemodule
+from exampleglobalmeasurementsensor import ExampleGlobalMeasurementSensor
+
+
+class Drivetrain:
+    """Represents a swerve drive style drivetrain."""
+
+    kMaxVelocity = 3.0  # 3 meters per second
+    kMaxAngularVelocity = math.pi  # 1/2 rotation per second
+
+    def __init__(self) -> None:
+        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)
+        self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7)
+        self.backLeft = swervemodule.SwerveModule(5, 6, 8, 9, 10, 11)
+        self.backRight = swervemodule.SwerveModule(7, 8, 12, 13, 14, 15)
+
+        self.imu = wpilib.OnboardIMU(wpilib.OnboardIMU.MountOrientation.FLAT)
+
+        self.kinematics = wpimath.SwerveDrive4Kinematics(
+            self.frontLeftLocation,
+            self.frontRightLocation,
+            self.backLeftLocation,
+            self.backRightLocation,
+        )
+
+        # Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers
+        # used below are robot specific, and should be tuned.
+        self.poseEstimator = wpimath.SwerveDrive4PoseEstimator(
+            self.kinematics,
+            self.imu.getRotation2d(),
+            (
+                self.frontLeft.getPosition(),
+                self.frontRight.getPosition(),
+                self.backLeft.getPosition(),
+                self.backRight.getPosition(),
+            ),
+            wpimath.Pose2d(),
+            (0.05, 0.05, wpimath.units.degreesToRadians(5)),
+            (0.5, 0.5, wpimath.units.degreesToRadians(30)),
+        )
+
+        self.imu.resetYaw()
+
+    def drive(
+        self,
+        xVelocity: float,
+        yVelocity: float,
+        rot: float,
+        fieldRelative: bool,
+        period: float,
+    ) -> None:
+        """Method to drive the robot using joystick info.
+
+        :param xVelocity: Velocity of the robot in the x direction (forward).
+        :param yVelocity: Velocity of the robot in the y direction (sideways).
+        :param rot: Angular rate of the robot.
+        :param fieldRelative: Whether the provided x and y velocities are relative to the field.
+        """
+        chassisVelocities = wpimath.ChassisVelocities(xVelocity, yVelocity, rot)
+        if fieldRelative:
+            chassisVelocities = chassisVelocities.toRobotRelative(
+                self.poseEstimator.getEstimatedPosition().rotation()
+            )
+
+        chassisVelocities = chassisVelocities.discretize(period)
+
+        velocities = wpimath.SwerveDrive4Kinematics.desaturateWheelVelocities(
+            self.kinematics.toSwerveModuleVelocities(chassisVelocities),
+            self.kMaxVelocity,
+        )
+
+        self.frontLeft.setDesiredVelocity(velocities[0])
+        self.frontRight.setDesiredVelocity(velocities[1])
+        self.backLeft.setDesiredVelocity(velocities[2])
+        self.backRight.setDesiredVelocity(velocities[3])
+
+    def updateOdometry(self) -> None:
+        """Updates the field relative position of the robot."""
+        self.poseEstimator.update(
+            self.imu.getRotation2d(),
+            (
+                self.frontLeft.getPosition(),
+                self.frontRight.getPosition(),
+                self.backLeft.getPosition(),
+                self.backRight.getPosition(),
+            ),
+        )
+
+        # Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
+        # a real robot, this must be calculated based either on latency or timestamps.
+        self.poseEstimator.addVisionMeasurement(
+            ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
+                self.poseEstimator.getEstimatedPosition()
+            ),
+            wpilib.Timer.getTimestamp() - 0.3,
+        )

robotpyExamples/examples/SwerveDrivePoseEstimator/exampleglobalmeasurementsensor.py

@@ -0,0 +1,33 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import random
+
+import wpimath
+
+
+class ExampleGlobalMeasurementSensor:
+    """This dummy class represents a global measurement sensor, such as a computer vision
+    solution.
+    """
+
+    def __init__(self) -> None:
+        raise RuntimeError("Utility class")
+
+    @staticmethod
+    def getEstimatedGlobalPose(estimatedRobotPose: wpimath.Pose2d) -> wpimath.Pose2d:
+        """Get a "noisy" fake global pose reading.
+
+        :param estimatedRobotPose: The robot pose.
+        """
+        rand_x = random.gauss(0.0, 0.5)
+        rand_y = random.gauss(0.0, 0.5)
+        rand_rot = random.gauss(0.0, wpimath.units.degreesToRadians(30))
+        return wpimath.Pose2d(
+            estimatedRobotPose.x + rand_x,
+            estimatedRobotPose.y + rand_y,
+            estimatedRobotPose.rotation().rotateBy(wpimath.Rotation2d(rand_rot)),
+        )

robotpyExamples/examples/SwerveDrivePoseEstimator/robot.py

@@ -0,0 +1,52 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import wpimath
+
+from drivetrain import Drivetrain
+
+
+class MyRobot(wpilib.TimedRobot):
+    def __init__(self) -> None:
+        super().__init__()
+
+        self.controller = wpilib.NiDsXboxController(0)
+        self.swerve = Drivetrain()
+
+        # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+        self.xvelocityLimiter = wpimath.SlewRateLimiter(3)
+        self.yvelocityLimiter = wpimath.SlewRateLimiter(3)
+        self.rotLimiter = wpimath.SlewRateLimiter(3)
+
+    def autonomousPeriodic(self) -> None:
+        self.driveWithJoystick(False)
+        self.swerve.updateOdometry()
+
+    def teleopPeriodic(self) -> None:
+        self.driveWithJoystick(True)
+
+    def driveWithJoystick(self, fieldRelative: bool) -> None:
+        # Get the x velocity. We are inverting this because Xbox controllers return
+        # negative values when we push forward.
+        xVelocity = -self.xvelocityLimiter.calculate(self.controller.getLeftY())
+        xVelocity *= Drivetrain.kMaxVelocity
+
+        # Get the y velocity or sideways/strafe velocity. We are inverting this because
+        # we want a positive value when we pull to the left. Xbox controllers
+        # return positive values when you pull to the right by default.
+        yVelocity = -self.yvelocityLimiter.calculate(self.controller.getLeftX())
+        yVelocity *= Drivetrain.kMaxVelocity
+
+        # Get the rate of angular rotation. We are inverting this because we want a
+        # positive value when we pull to the left (remember, CCW is positive in
+        # mathematics). Xbox controllers return positive values when you pull to
+        # the right by default.
+        rot = -self.rotLimiter.calculate(self.controller.getRightX())
+        rot *= Drivetrain.kMaxAngularVelocity
+
+        self.swerve.drive(xVelocity, yVelocity, rot, fieldRelative, self.getPeriod())

robotpyExamples/examples/SwerveDrivePoseEstimator/swervemodule.py

@@ -0,0 +1,128 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+
+import wpilib
+import wpimath
+
+kWheelRadius = 0.0508
+kEncoderResolution = 4096
+
+kModuleMaxAngularVelocity = math.pi
+kModuleMaxAngularAcceleration = math.tau  # radians per second squared
+
+
+class SwerveModule:
+    def __init__(
+        self,
+        driveMotorChannel: int,
+        turningMotorChannel: int,
+        driveEncoderChannelA: int,
+        driveEncoderChannelB: int,
+        turningEncoderChannelA: int,
+        turningEncoderChannelB: int,
+    ) -> None:
+        """Constructs a SwerveModule with a drive motor, turning motor, drive encoder and
+        turning encoder.
+
+        :param driveMotorChannel: PWM output for the drive motor.
+        :param turningMotorChannel: PWM output for the turning motor.
+        :param driveEncoderChannelA: DIO input for the drive encoder channel A
+        :param driveEncoderChannelB: DIO input for the drive encoder channel B
+        :param turningEncoderChannelA: DIO input for the turning encoder channel A
+        :param turningEncoderChannelB: DIO input for the turning encoder channel B
+        """
+        self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel)
+        self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel)
+
+        self.driveEncoder = wpilib.Encoder(driveEncoderChannelA, driveEncoderChannelB)
+        self.turningEncoder = wpilib.Encoder(
+            turningEncoderChannelA, turningEncoderChannelB
+        )
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.drivePIDController = wpimath.PIDController(1, 0, 0)
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.turningPIDController = wpimath.ProfiledPIDController(
+            1,
+            0,
+            0,
+            wpimath.TrapezoidProfile.Constraints(
+                kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration
+            ),
+        )
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.driveFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3)
+        self.turnFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 0.5)
+
+        # 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
+        # resolution.
+        self.driveEncoder.setDistancePerPulse(
+            math.tau * kWheelRadius / kEncoderResolution
+        )
+
+        # Set the distance (in this case, angle) in radians per pulse for the turning encoder.
+        # This is the the angle through an entire rotation (2 * pi) divided by the
+        # encoder resolution.
+        self.turningEncoder.setDistancePerPulse(math.tau / kEncoderResolution)
+
+        # Limit the PID Controller's input range between -pi and pi and set the input
+        # to be continuous.
+        self.turningPIDController.enableContinuousInput(-math.pi, math.pi)
+
+    def getState(self) -> wpimath.SwerveModuleVelocity:
+        """Returns the current state of the module.
+
+        :returns: The current state of the module.
+        """
+        return wpimath.SwerveModuleVelocity(
+            self.driveEncoder.getRate(),
+            wpimath.Rotation2d(self.turningEncoder.getDistance()),
+        )
+
+    def getPosition(self) -> wpimath.SwerveModulePosition:
+        """Returns the current position of the module.
+
+        :returns: The current position of the module.
+        """
+        return wpimath.SwerveModulePosition(
+            self.driveEncoder.getDistance(),
+            wpimath.Rotation2d(self.turningEncoder.getDistance()),
+        )
+
+    def setDesiredVelocity(self, desiredVelocity: wpimath.SwerveModuleVelocity) -> None:
+        """Sets the desired state for the module.
+
+        :param desiredVelocity: Desired state with velocity and angle.
+        """
+        encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance())
+
+        # Optimize the desired velocity to avoid spinning further than 90 degrees, then scale
+        # velocity 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.
+        velocity = desiredVelocity.optimize(encoderRotation).cosineScale(
+            encoderRotation
+        )
+
+        # Calculate the drive output from the drive PID controller and feedforward.
+        driveOutput = self.drivePIDController.calculate(
+            self.driveEncoder.getRate(), velocity.velocity
+        ) + self.driveFeedforward.calculate(velocity.velocity)
+
+        # Calculate the turning motor output from the turning PID controller and feedforward.
+        turnOutput = self.turningPIDController.calculate(
+            self.turningEncoder.getDistance(), velocity.angle.radians()
+        ) + self.turnFeedforward.calculate(
+            self.turningPIDController.getSetpoint().velocity
+        )
+
+        self.driveMotor.setVoltage(driveOutput)
+        self.turningMotor.setVoltage(turnOutput)

robotpyExamples/examples/SysId/constants.py

@@ -0,0 +1,73 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+
+import wpimath.units
+
+
+class DriveConstants:
+    kLeftMotor1Port = 0
+    kLeftMotor2Port = 1
+    kRightMotor1Port = 2
+    kRightMotor2Port = 3
+
+    kLeftEncoderPorts = (0, 1)
+    kRightEncoderPorts = (2, 3)
+    kLeftEncoderReversed = False
+    kRightEncoderReversed = True
+
+    kEncoderCPR = 1024
+    kWheelDiameter = wpimath.units.inchesToMeters(6)
+    # Assumes the encoders are directly mounted on the wheel shafts
+    kEncoderDistancePerPulse = (kWheelDiameter * math.pi) / kEncoderCPR
+
+
+class ShooterConstants:
+    kEncoderPorts = (4, 5)
+    kEncoderReversed = False
+    kEncoderCPR = 1024
+    # Distance units will be rotations
+    kEncoderDistancePerPulse = 1.0 / kEncoderCPR
+
+    kShooterMotorPort = 4
+    kFeederMotorPort = 5
+
+    kShooterFreeRPS = 5300.0
+    kShooterTargetRPS = 4000.0
+    kShooterToleranceRPS = 50.0
+
+    # These are not real PID gains, and will have to be tuned for your specific robot.
+    kP = 1.0
+
+    # On a real robot the feedforward constants should be empirically determined; these are
+    # reasonable guesses.
+    kS = 0.05  # V
+    # Should have value 12V at free velocity
+    kV = 12.0 / kShooterFreeRPS  # V/(rot/s)
+    kA = 0.0  # V/(rot/s^2)
+
+    kFeederVelocity = 0.5
+
+
+class IntakeConstants:
+    kMotorPort = 6
+    kSolenoidPorts = (2, 3)
+
+
+class StorageConstants:
+    kMotorPort = 7
+    kBallSensorPort = 6
+
+
+class AutoConstants:
+    kTimeout = 3
+    kDriveDistance = 2.0  # m
+    kDriveVelocity = 0.5
+
+
+class OIConstants:
+    kDriverControllerPort = 0

robotpyExamples/examples/SysId/robot.py

@@ -0,0 +1,66 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from commands2 import CommandScheduler, TimedCommandRobot
+
+from sysidroutinebot import SysIdRoutineBot
+
+
+class MyRobot(TimedCommandRobot):
+    """
+    The methods in this class are called automatically corresponding to each mode,
+    as described in the TimedRobot documentation.
+    """
+
+    def __init__(self) -> None:
+        """This function is run when the robot is first started up and should be used for any
+        initialization code.
+        """
+        super().__init__()
+        self.robot = SysIdRoutineBot()
+
+        # Configure default commands and condition bindings on robot startup
+        self.robot.configureBindings()
+
+        self.autonomous_command = None
+
+    def disabledInit(self) -> None:
+        """This function is called once each time the robot enters Disabled mode."""
+        pass
+
+    def disabledPeriodic(self) -> None:
+        pass
+
+    def autonomousInit(self) -> None:
+        self.autonomous_command = self.robot.getAutonomousCommand()
+
+        if self.autonomous_command is not None:
+            CommandScheduler.getInstance().schedule(self.autonomous_command)
+
+    def autonomousPeriodic(self) -> None:
+        """This function is called periodically during autonomous."""
+        pass
+
+    def teleopInit(self) -> None:
+        # This makes sure that the autonomous stops running when
+        # teleop starts running. If you want the autonomous to
+        # continue until interrupted by another command, remove
+        # this line or comment it out.
+        if self.autonomous_command is not None:
+            self.autonomous_command.cancel()
+
+    def teleopPeriodic(self) -> None:
+        """This function is called periodically during operator control."""
+        pass
+
+    def utilityInit(self) -> None:
+        # Cancels all running commands at the start of utility mode.
+        CommandScheduler.getInstance().cancelAll()
+
+    def utilityPeriodic(self) -> None:
+        """This function is called periodically during utility mode."""
+        pass

robotpyExamples/examples/SysId/subsystems/drive.py

@@ -0,0 +1,125 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from typing import Callable
+
+from commands2 import Command, Subsystem
+from commands2.sysid import SysIdRoutine
+from wpilib import DifferentialDrive, Encoder, PWMSparkMax, RobotController
+from wpilib.sysid import SysIdRoutineLog
+
+from constants import DriveConstants
+
+
+class Drive(Subsystem):
+    """Represents the drive subsystem."""
+
+    def __init__(self) -> None:
+        """Creates a new Drive subsystem."""
+        super().__init__()
+
+        # The motors on the left side of the drive.
+        self.left_motor = PWMSparkMax(DriveConstants.kLeftMotor1Port)
+
+        # The motors on the right side of the drive.
+        self.right_motor = PWMSparkMax(DriveConstants.kRightMotor1Port)
+
+        # The robot's drive
+        self.drive = DifferentialDrive(self.left_motor, self.right_motor)
+
+        # The left-side drive encoder
+        self.left_encoder = Encoder(
+            DriveConstants.kLeftEncoderPorts[0],
+            DriveConstants.kLeftEncoderPorts[1],
+            DriveConstants.kLeftEncoderReversed,
+        )
+
+        # The right-side drive encoder
+        self.right_encoder = Encoder(
+            DriveConstants.kRightEncoderPorts[0],
+            DriveConstants.kRightEncoderPorts[1],
+            DriveConstants.kRightEncoderReversed,
+        )
+
+        # Create a new SysId routine for characterizing the drive.
+        self.sys_id_routine = SysIdRoutine(
+            # Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage.
+            SysIdRoutine.Config(),
+            SysIdRoutine.Mechanism(
+                # Tell SysId how to plumb the driving voltage to the motors.
+                self._driveVoltage,
+                # Tell SysId how to record a frame of data for each motor on the mechanism being
+                # characterized.
+                self._log,
+                # Tell SysId to make generated commands require this subsystem, suffix test state in
+                # WPILog with this subsystem's name ("drive")
+                self,
+            ),
+        )
+
+        # Add the second motors on each side of the drivetrain
+        self.left_motor.addFollower(PWMSparkMax(DriveConstants.kLeftMotor2Port))
+        self.right_motor.addFollower(PWMSparkMax(DriveConstants.kRightMotor2Port))
+
+        # We need to invert one side of the drivetrain so that positive voltages
+        # result in both sides moving forward. Depending on how your robot's
+        # gearbox is constructed, you might have to invert the left side instead.
+        self.right_motor.setInverted(True)
+
+        # Sets the distance per pulse for the encoders
+        self.left_encoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse)
+        self.right_encoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse)
+
+    def _driveVoltage(self, voltage: float) -> None:
+        self.left_motor.setVoltage(voltage)
+        self.right_motor.setVoltage(voltage)
+
+    def _log(self, sys_id_routine: SysIdRoutineLog) -> None:
+        # Record a frame for the left motors. Since these share an encoder, we consider
+        # the entire group to be one motor.
+        sys_id_routine.motor("drive-left").voltage(
+            self.left_motor.get() * RobotController.getBatteryVoltage()
+        ).position(self.left_encoder.getDistance()).velocity(
+            self.left_encoder.getRate()
+        )
+        # Record a frame for the right motors. Since these share an encoder, we consider
+        # the entire group to be one motor.
+        sys_id_routine.motor("drive-right").voltage(
+            self.right_motor.get() * RobotController.getBatteryVoltage()
+        ).position(self.right_encoder.getDistance()).velocity(
+            self.right_encoder.getRate()
+        )
+
+    def arcadeDriveCommand(
+        self, fwd: Callable[[], float], rot: Callable[[], float]
+    ) -> Command:
+        """Returns a command that drives the robot with arcade controls.
+
+        :param fwd: the commanded forward movement
+        :param rot: the commanded rotation
+        """
+
+        # A split-stick arcade command, with forward/backward controlled by the left
+        # hand, and turning controlled by the right.
+        return self.run(lambda: self.drive.arcadeDrive(fwd(), rot())).withName(
+            "arcadeDrive"
+        )
+
+    def sysIdQuasistatic(self, direction: SysIdRoutine.Direction) -> Command:
+        """Returns a command that will execute a quasistatic test in the given direction.
+
+        :param direction: The direction (forward or reverse) to run the test in
+        """
+
+        return self.sys_id_routine.quasistatic(direction)
+
+    def sysIdDynamic(self, direction: SysIdRoutine.Direction) -> Command:
+        """Returns a command that will execute a dynamic test in the given direction.
+
+        :param direction: The direction (forward or reverse) to run the test in
+        """
+
+        return self.sys_id_routine.dynamic(direction)

robotpyExamples/examples/SysId/subsystems/shooter.py

@@ -0,0 +1,118 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from typing import Callable
+
+from commands2 import Command, Subsystem
+from commands2.sysid import SysIdRoutine
+from wpilib import Encoder, PWMSparkMax, RobotController
+from wpilib.sysid import SysIdRoutineLog
+
+import wpimath
+import wpimath.units
+
+from constants import ShooterConstants
+
+
+class Shooter(Subsystem):
+    """Represents the shooter subsystem."""
+
+    def __init__(self) -> None:
+        """Creates a new Shooter subsystem."""
+        super().__init__()
+
+        # The motor on the shooter wheel .
+        self.shooter_motor = PWMSparkMax(ShooterConstants.kShooterMotorPort)
+
+        # The motor on the feeder wheels.
+        self.feeder_motor = PWMSparkMax(ShooterConstants.kFeederMotorPort)
+
+        # The shooter wheel encoder
+        self.shooter_encoder = Encoder(
+            ShooterConstants.kEncoderPorts[0],
+            ShooterConstants.kEncoderPorts[1],
+            ShooterConstants.kEncoderReversed,
+        )
+
+        # Create a new SysId routine for characterizing the shooter.
+        self.sys_id_routine = SysIdRoutine(
+            # Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage.
+            SysIdRoutine.Config(),
+            SysIdRoutine.Mechanism(
+                # Tell SysId how to plumb the driving voltage to the motor(s).
+                self.shooter_motor.setVoltage,
+                # Tell SysId how to record a frame of data for each motor on the mechanism being
+                # characterized.
+                self._log,
+                # Tell SysId to make generated commands require this subsystem, suffix test state in
+                # WPILog with this subsystem's name ("shooter")
+                self,
+            ),
+        )
+
+        # PID controller to run the shooter wheel in closed-loop, set the constants equal to those
+        # calculated by SysId
+        self.shooter_feedback = wpimath.PIDController(ShooterConstants.kP, 0, 0)
+        # Feedforward controller to run the shooter wheel in closed-loop, set the constants equal to
+        # those calculated by SysId
+        self.shooter_feedforward = wpimath.SimpleMotorFeedforwardRadians(
+            ShooterConstants.kS,
+            ShooterConstants.kV / wpimath.units.rotationsToRadians(1),
+            ShooterConstants.kA / wpimath.units.rotationsToRadians(1),
+        )
+
+        # Sets the distance per pulse for the encoders
+        self.shooter_encoder.setDistancePerPulse(
+            ShooterConstants.kEncoderDistancePerPulse
+        )
+
+    def _log(self, sys_id_routine: SysIdRoutineLog) -> None:
+        # Record a frame for the shooter motor.
+        sys_id_routine.motor("shooter-wheel").voltage(
+            self.shooter_motor.get() * RobotController.getBatteryVoltage()
+        ).angularPosition(self.shooter_encoder.getDistance()).angularVelocity(
+            self.shooter_encoder.getRate()
+        )
+
+    def runShooter(self, shooterVelocity: Callable[[], float]) -> Command:
+        """Returns a command that runs the shooter at a specifc velocity.
+
+        :param shooterVelocity: The commanded shooter wheel velocity in rotations per second
+        """
+
+        # Run shooter wheel at the desired velocity using a PID controller and feedforward.
+        def _run_shooter() -> None:
+            target_velocity = shooterVelocity()
+            target_velocity_radians = wpimath.units.rotationsToRadians(target_velocity)
+            self.shooter_motor.setVoltage(
+                self.shooter_feedback.calculate(
+                    self.shooter_encoder.getRate(), target_velocity
+                )
+                + self.shooter_feedforward.calculate(target_velocity_radians)
+            )
+            self.feeder_motor.setThrottle(ShooterConstants.kFeederVelocity)
+
+        def _stop_motors(interrupted: bool) -> None:
+            self.shooter_motor.stopMotor()
+            self.feeder_motor.stopMotor()
+
+        return self.run(_run_shooter).finallyDo(_stop_motors).withName("runShooter")
+
+    def sysIdQuasistatic(self, direction: SysIdRoutine.Direction) -> Command:
+        """Returns a command that will execute a quasistatic test in the given direction.
+
+        :param direction: The direction (forward or reverse) to run the test in
+        """
+
+        return self.sys_id_routine.quasistatic(direction)
+
+    def sysIdDynamic(self, direction: SysIdRoutine.Direction) -> Command:
+        """Returns a command that will execute a dynamic test in the given direction.
+
+        :param direction: The direction (forward or reverse) to run the test in
+        """
+
+        return self.sys_id_routine.dynamic(direction)

robotpyExamples/examples/SysId/sysidroutinebot.py

@@ -0,0 +1,91 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from commands2 import Command
+from commands2.button import CommandNiDsXboxController
+from commands2.sysid import SysIdRoutine
+
+from subsystems.drive import Drive
+from subsystems.shooter import Shooter
+
+from constants import OIConstants
+
+
+class SysIdRoutineBot:
+    """This class is where the bulk of the robot should be declared. Since Command-based is a
+    "declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot`
+    periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+    subsystems, commands, and button mappings) should be declared here.
+    """
+
+    def __init__(self) -> None:
+        # The robot's subsystems
+        self.drive = Drive()
+        self.shooter = Shooter()
+
+        # The driver's controller
+        self.controller = CommandNiDsXboxController(OIConstants.kDriverControllerPort)
+
+    def configureBindings(self) -> None:
+        """Use this method to define bindings between conditions and commands. These are useful for
+        automating robot behaviors based on button and sensor input.
+
+        Should be called in the robot class constructor.
+
+        Event binding methods are available on the :class:`.Trigger` class.
+        """
+
+        # Control the drive with split-stick arcade controls
+        self.drive.setDefaultCommand(
+            self.drive.arcadeDriveCommand(
+                lambda: -self.controller.getLeftY(),
+                lambda: -self.controller.getRightX(),
+            )
+        )
+
+        # Bind full set of SysId routine tests to buttons; a complete routine should run each of these
+        # once.
+        # Using bumpers as a modifier and combining it with the buttons so that we can have both sets
+        # of bindings at once
+        self.controller.a().and_(self.controller.rightBumper()).whileTrue(
+            self.drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)
+        )
+        self.controller.b().and_(self.controller.rightBumper()).whileTrue(
+            self.drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)
+        )
+        self.controller.x().and_(self.controller.rightBumper()).whileTrue(
+            self.drive.sysIdDynamic(SysIdRoutine.Direction.kForward)
+        )
+        self.controller.y().and_(self.controller.rightBumper()).whileTrue(
+            self.drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)
+        )
+
+        # Control the shooter wheel with the left trigger
+        self.shooter.setDefaultCommand(
+            self.shooter.runShooter(self.controller.getLeftTriggerAxis)
+        )
+
+        self.controller.a().and_(self.controller.leftBumper()).whileTrue(
+            self.shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward)
+        )
+        self.controller.b().and_(self.controller.leftBumper()).whileTrue(
+            self.shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)
+        )
+        self.controller.x().and_(self.controller.leftBumper()).whileTrue(
+            self.shooter.sysIdDynamic(SysIdRoutine.Direction.kForward)
+        )
+        self.controller.y().and_(self.controller.leftBumper()).whileTrue(
+            self.shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse)
+        )
+
+    def getAutonomousCommand(self) -> Command:
+        """Use this to define the command that runs during autonomous.
+
+        Scheduled during :meth:`.Robot.autonomousInit`.
+        """
+
+        # Do nothing
+        return self.drive.run(lambda: None)

robotpyExamples/examples/TankDrive/robot.py

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    This is a demo program showing the use of the DifferentialDrive class.
+    Runs the motors with tank steering.
+    """
+
+    def __init__(self):
+        """Robot initialization function"""
+        super().__init__()
+
+        leftMotor = wpilib.PWMSparkMax(0)
+        rightMotor = wpilib.PWMSparkMax(1)
+        self.robotDrive = wpilib.DifferentialDrive(leftMotor, rightMotor)
+        self.leftStick = wpilib.Joystick(0)
+        self.rightStick = wpilib.Joystick(1)
+
+        # We need to invert one side of the drivetrain so that positive voltages
+        # result in both sides moving forward. Depending on how your robot's
+        # gearbox is constructed, you might have to invert the left side instead.
+        rightMotor.setInverted(True)
+
+    def teleopPeriodic(self):
+        self.robotDrive.tankDrive(-self.leftStick.getY(), -self.rightStick.getY())

robotpyExamples/examples/TankDriveXboxController/robot.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    This is a demo program showing the use of the DifferentialDrive class.
+    Runs the motors with tank steering and an Xbox controller.
+    """
+
+    def __init__(self):
+        """Robot initialization function"""
+        super().__init__()
+
+        leftMotor = wpilib.PWMSparkMax(0)
+        rightMotor = wpilib.PWMSparkMax(1)
+        self.robotDrive = wpilib.DifferentialDrive(leftMotor, rightMotor)
+        self.driverController = wpilib.NiDsXboxController(0)
+
+        # We need to invert one side of the drivetrain so that positive voltages
+        # result in both sides moving forward. Depending on how your robot's
+        # gearbox is constructed, you might have to invert the left side instead.
+        rightMotor.setInverted(True)
+
+    def teleopPeriodic(self):
+        # Drive with tank drive.
+        # That means that the Y axis of the left stick moves the left side
+        # of the robot forward and backward, and the Y axis of the right stick
+        # moves the right side of the robot forward and backward.
+        self.robotDrive.tankDrive(
+            -self.driverController.getLeftY(), -self.driverController.getRightY()
+        )

robotpyExamples/examples/UnitTest/constants.py

@@ -0,0 +1,16 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+
+class IntakeConstants:
+    MOTOR_PORT = 1
+
+    PISTON_FWD_CHANNEL = 0
+    PISTON_REV_CHANNEL = 1
+    INTAKE_VELOCITY = 0.5
+
+
+JOYSTICK_INDEX = 0

robotpyExamples/examples/UnitTest/robot.py

@@ -0,0 +1,39 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+
+import constants
+from subsystems.intake import Intake
+
+
+class MyRobot(wpilib.TimedRobot):
+    """The methods in this class are called automatically corresponding to each mode, as
+    described in the TimedRobot documentation. If you change the name of this class or the
+    package after creating this project, you must also update the Main.java file in the
+    project.
+    """
+
+    def __init__(self) -> None:
+        super().__init__()
+        self.intake = Intake()
+        self.joystick = wpilib.Joystick(constants.JOYSTICK_INDEX)
+
+    def teleopPeriodic(self) -> None:
+        """This function is called periodically during operator control."""
+        # Activate the intake while the trigger is held
+        if self.joystick.getTrigger():
+            self.intake.activate(constants.IntakeConstants.INTAKE_VELOCITY)
+        else:
+            self.intake.activate(0)
+
+        # Toggle deploying the intake when the top button is pressed
+        if self.joystick.getTop():
+            if self.intake.isDeployed():
+                self.intake.retract()
+            else:
+                self.intake.deploy()

robotpyExamples/examples/UnitTest/subsystems/intake.py

@@ -0,0 +1,36 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+
+from constants import IntakeConstants
+
+
+class Intake:
+    def __init__(self) -> None:
+        self.motor = wpilib.PWMSparkMax(IntakeConstants.MOTOR_PORT)
+        self.piston = wpilib.DoubleSolenoid(
+            0,
+            wpilib.PneumaticsModuleType.CTRE_PCM,
+            IntakeConstants.PISTON_FWD_CHANNEL,
+            IntakeConstants.PISTON_REV_CHANNEL,
+        )
+
+    def deploy(self) -> None:
+        self.piston.setThrottle(wpilib.DoubleSolenoid.Value.FORWARD)
+
+    def retract(self) -> None:
+        self.piston.setThrottle(wpilib.DoubleSolenoid.Value.REVERSE)
+        self.motor.setThrottle(0)  # turn off the motor
+
+    def activate(self, velocity: float) -> None:
+        if self.isDeployed():
+            self.motor.setThrottle(velocity)
+        else:  # if piston isn't open, do nothing
+            self.motor.setThrottle(0)
+
+    def isDeployed(self) -> bool:
+        return self.piston.get() == wpilib.DoubleSolenoid.Value.FORWARD

robotpyExamples/examples/XrpReference/commands/arcadedrive.py

@@ -0,0 +1,34 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import typing
+import commands2
+from subsystems.drivetrain import Drivetrain
+
+
+class ArcadeDrive(commands2.Command):
+    def __init__(
+        self,
+        drive: Drivetrain,
+        xaxisVelocitySupplier: typing.Callable[[], float],
+        zaxisRotateSupplier: typing.Callable[[], float],
+    ) -> None:
+        """Creates a new ArcadeDrive. This command will drive your robot according to the velocity supplier
+        lambdas. This command does not terminate.
+
+        :param drivetrain:  The drivetrain subsystem on which this command will run
+        :param xaxisVelocitySupplier:     Callable supplier of forward/backward velocity
+        :param zaxisRotateSupplier:       Callable supplier of rotational velocity
+        """
+
+        self.drive = drive
+        self.xaxisVelocitySupplier = xaxisVelocitySupplier
+        self.zaxisRotateSupplier = zaxisRotateSupplier
+
+        self.addRequirements(self.drive)
+
+    def execute(self) -> None:
+        self.drive.arcadeDrive(self.xaxisVelocitySupplier(), self.zaxisRotateSupplier())

robotpyExamples/examples/XrpReference/commands/autonomousdistance.py

@@ -0,0 +1,28 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+
+from commands.drivedistance import DriveDistance
+from commands.turndegrees import TurnDegrees
+from subsystems.drivetrain import Drivetrain
+
+
+class AutonomousDistance(commands2.SequentialCommandGroup):
+    def __init__(self, drive: Drivetrain) -> None:
+        """Creates a new Autonomous Drive based on distance. This will drive out for a specified distance,
+        turn around and drive back.
+
+        :param drivetrain: The drivetrain subsystem on which this command will run
+        """
+        super().__init__()
+
+        self.addCommands(
+            DriveDistance(-0.5, 10, drive),
+            TurnDegrees(-0.5, 180, drive),
+            DriveDistance(-0.5, 10, drive),
+            TurnDegrees(0.5, 180, drive),
+        )

robotpyExamples/examples/XrpReference/commands/autonomoustime.py

@@ -0,0 +1,30 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import commands2
+
+from commands.drivetime import DriveTime
+from commands.turntime import TurnTime
+from subsystems.drivetrain import Drivetrain
+
+
+class AutonomousTime(commands2.SequentialCommandGroup):
+    def __init__(self, drive: Drivetrain) -> None:
+        """Creates a new Autonomous Drive based on time. This will drive out for a period of time, turn
+        around for time (equivalent to time to turn around) and drive forward again. This should mimic
+        driving out, turning around and driving back.
+
+        :param drivetrain: The drive subsystem on which this command will run
+
+        """
+        super().__init__()
+
+        self.addCommands(
+            DriveTime(-0.6, 2.0, drive),
+            TurnTime(-0.5, 1.3, drive),
+            DriveTime(-0.6, 2.0, drive),
+            TurnTime(0.5, 1.3, drive),
+        )