[copybara] Import robotpy examples (#8608)

GitOrigin-RevId: 9ba4bc3040fa7e772f5a594039e78fc6c43d114e

.wpiformat

@@ -14,6 +14,7 @@ generatedFileExclude {
   generated/
   hal/src/main/native/systemcore/rev/
   python/
+  robotpyExamples/
   resources/
   thirdparty/
   wpigui/src/main/native/cpp/portable-file-dialogs\.cpp$

commandsv2/src/main/python/commands2/button/commandnidsxboxcontroller.py

@@ -44,7 +44,7 @@ class CommandNiDsXboxController(CommandGenericHID):
         """
         if loop is None:
             loop = CommandScheduler.getInstance().getDefaultButtonLoop()
-        return Trigger(loop, lambda: self._hid.getLeftBumper())
+        return Trigger(loop, lambda: self._hid.getLeftBumperButton())
 
     def rightBumper(self, loop: Optional[EventLoop] = None) -> Trigger:
         """
@@ -58,7 +58,7 @@ class CommandNiDsXboxController(CommandGenericHID):
         """
         if loop is None:
             loop = CommandScheduler.getInstance().getDefaultButtonLoop()
-        return Trigger(loop, lambda: self._hid.getRightBumper())
+        return Trigger(loop, lambda: self._hid.getRightBumperButton())
 
     def leftStick(self, loop: Optional[EventLoop] = None) -> Trigger:
         """

robotpyExamples/AddressableLED/robot.py

@@ -0,0 +1,45 @@
+#!/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.units
+
+
+class MyRobot(wpilib.TimedRobot):
+    def __init__(self) -> None:
+        super().__init__()
+
+        # SmartIO port 1
+        self.led = wpilib.AddressableLED(1)
+
+        # Reuse buffer
+        # Default to a length of 60
+        self.ledData = [wpilib.AddressableLED.LEDData() for _ in range(60)]
+        self.led.setLength(len(self.ledData))
+
+        # Set the data
+        self.led.setData(self.ledData)
+
+        # Create an LED pattern that will display a rainbow across
+        # all hues at maximum saturation and half brightness
+        self.rainbow = wpilib.LEDPattern.rainbow(255, 128)
+
+        # Our LED strip has a density of 120 LEDs per meter
+        self.kLedSpacing = 1 / 120.0
+
+        # Create a new pattern that scrolls the rainbow pattern across the LED strip, moving at a
+        # speed of 1 meter per second.
+        self.scrollingRainbow = self.rainbow.scrollAtAbsoluteSpeed(
+            1,
+            self.kLedSpacing,
+        )
+
+    def robotPeriodic(self) -> None:
+        # Update the buffer with the rainbow animation
+        self.scrollingRainbow.applyTo(self.ledData)
+        # Set the LEDs
+        self.led.setData(self.ledData)

robotpyExamples/AprilTagsVision/robot.py

@@ -0,0 +1,26 @@
+#!/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 wpilib.cameraserver
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    This is a demo program showing the detection of AprilTags. The image is acquired from the USB
+    camera, then any detected AprilTags are marked up on the image and sent to the dashboard.
+    Be aware that the performance on this is much worse than a coprocessor solution!
+    """
+
+    def __init__(self):
+        super().__init__()
+        # Your image processing code will be launched via a stub that will set up logging and initialize NetworkTables
+        # to talk to your robot code.
+        # https://robotpy.readthedocs.io/en/stable/vision/roborio.html#important-notes
+
+        wpilib.CameraServer.launch("vision.py:main")

robotpyExamples/AprilTagsVision/vision.py

@@ -0,0 +1,151 @@
+#
+# 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 robotpy_apriltag
+from cscore import CameraServer
+
+import cv2
+import numpy as np
+
+#
+# This code will work both on a RoboRIO and on other platforms. The exact mechanism
+# to run it differs depending on whether you’re on a RoboRIO or a coprocessor
+#
+# https://robotpy.readthedocs.io/en/stable/vision/code.html
+
+
+def main():
+    detector = robotpy_apriltag.AprilTagDetector()
+
+    # look for tag36h11, correct 1 error bit (hamming distance 1)
+    # hamming 1 allocates 781KB, 2 allocates 27.4 MB, 3 allocates 932 MB
+    # max of 1 recommended for RoboRIO 1, while hamming 2 is feasible on the RoboRIO 2
+    detector.addFamily("tag36h11", 1)
+
+    # Set up Pose Estimator - parameters are for a Microsoft Lifecam HD-3000
+    # (https://www.chiefdelphi.com/t/wpilib-apriltagdetector-sample-code/421411/21)
+    poseEstConfig = robotpy_apriltag.AprilTagPoseEstimator.Config(
+        0.1651,
+        699.3778103158814,
+        677.7161226393544,
+        345.6059345433618,
+        207.12741326228522,
+    )
+    estimator = robotpy_apriltag.AprilTagPoseEstimator(poseEstConfig)
+
+    # Get the UsbCamera from CameraServer
+    camera = CameraServer.startAutomaticCapture()
+
+    # Set the resolution
+    camera.setResolution(640, 480)
+
+    # Get a CvSink. This will capture Mats from the camera
+    cvSink = CameraServer.getVideo()
+
+    # Set up a CvSource. This will send images back to the Dashboard
+    outputStream = CameraServer.putVideo("Detected", 640, 480)
+
+    # Mats are very memory expensive. Let's reuse these.
+    mat = np.zeros((480, 640, 3), dtype=np.uint8)
+    grayMat = np.zeros(shape=(480, 640), dtype=np.uint8)
+
+    # Instantiate once
+    tags = []
+    outlineColor = (0, 255, 0)
+    crossColor = (0, 0, 255)
+
+    # Output the list to Network Tables
+    tagsTable = ntcore.NetworkTableInstance.getDefault().getTable("apriltags")
+    pubTags = tagsTable.getIntegerArrayTopic("tags").publish()
+
+    try:
+        while True:
+            # Tell the CvSink to grab a frame from the camera and put it
+            # in the source mat.  If there is an error notify the output.
+            if cvSink.grabFrame(mat) == 0:
+                # Send the output frame the error
+                outputStream.notifyError(cvSink.getError())
+
+                # Skip the rest of the current iteration
+                continue
+
+            cv2.cvtColor(mat, cv2.COLOR_RGB2GRAY, dst=grayMat)
+
+            detections = detector.detect(grayMat)
+
+            # have not seen any tags yet
+            tags.clear()
+
+            for detection in detections:
+                # remember we saw this tag
+                tags.append(detection.getId())
+
+                # draw lines around the tag
+                for i in range(4):
+                    j = (i + 1) % 4
+                    point1 = (
+                        int(detection.getCorner(i).x),
+                        int(detection.getCorner(i).y),
+                    )
+                    point2 = (
+                        int(detection.getCorner(j).x),
+                        int(detection.getCorner(j).y),
+                    )
+                    mat = cv2.line(mat, point1, point2, outlineColor, 2)
+
+                # mark the center of the tag
+                cx = int(detection.getCenter().x)
+                cy = int(detection.getCenter().y)
+                ll = 10
+                mat = cv2.line(
+                    mat,
+                    (cx - ll, cy),
+                    (cx + ll, cy),
+                    crossColor,
+                    2,
+                )
+                mat = cv2.line(
+                    mat,
+                    (cx, cy - ll),
+                    (cx, cy + ll),
+                    crossColor,
+                    2,
+                )
+
+                # identify the tag
+                mat = cv2.putText(
+                    mat,
+                    str(detection.getId()),
+                    (cx + ll, cy),
+                    cv2.FONT_HERSHEY_SIMPLEX,
+                    1,
+                    crossColor,
+                    3,
+                )
+
+                # determine pose
+                pose = estimator.estimate(detection)
+
+                # put pose into dashboard
+                rot = pose.rotation()
+                tagsTable.getEntry(f"pose_{detection.getId()}").setDoubleArray(
+                    [pose.X(), pose.Y(), pose.Z(), rot.X(), rot.Y(), rot.Z()]
+                )
+
+            # put list of tags onto dashboard
+            pubTags.set(tags)
+
+            # Give output stream a new image to display
+            outputStream.putFrame(mat)
+    finally:
+        pubTags.close()
+        detector.close()
+
+    # The camera code will be killed when the robot.py program exits. If you wish to perform cleanup,
+    # you should register an atexit handler. The child process will NOT be launched when running the robot code in
+    # simulation or unit testing mode

robotpyExamples/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/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/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/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/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.kYellow),
+        )
+
+        # 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.kBlue))
+
+        # 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.get() * 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.set(0.0)

robotpyExamples/CANPDP/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):
+    """
+    This is a sample program showing how to retrieve information from the Power
+    Distribution Panel via CAN. The information will be displayed under variables
+    through the SmartDashboard.
+    """
+
+    def __init__(self):
+        """Robot initialization function"""
+        super().__init__()
+
+        # Object for dealing with the Power Distribution Panel (PDP).
+        self.pdp = wpilib.PowerDistribution(0)
+
+        # Put the PDP itself to the dashboard
+        wpilib.SmartDashboard.putData("PDP", self.pdp)
+
+    def robotPeriodic(self):
+        # Get the current going through channel 7, in Amperes.
+        # The PDP returns the current in increments of 0.125A.
+        # At low currents the current readings tend to be less accurate.
+        current7 = self.pdp.getCurrent(7)
+        wpilib.SmartDashboard.putNumber("Current Channel 7", current7)
+
+        # Get the voltage going into the PDP, in Volts.
+        # The PDP returns the voltage in increments of 0.05 Volts.
+        voltage = self.pdp.getVoltage()
+        wpilib.SmartDashboard.putNumber("Voltage", voltage)
+
+        # Retrieves the temperature of the PDP, in degrees Celsius.
+        temperatureCelsius = self.pdp.getTemperature()
+        wpilib.SmartDashboard.putNumber("Temperature", temperatureCelsius)
+
+        # Get the total current of all channels.
+        totalCurrent = self.pdp.getTotalCurrent()
+        wpilib.SmartDashboard.putNumber("Total Current", totalCurrent)
+
+        # Get the total power of all channels.
+        # Power is the bus voltage multiplied by the current with the units Watts.
+        totalPower = self.pdp.getTotalPower()
+        wpilib.SmartDashboard.putNumber("Total Power", totalPower)
+
+        # Get the total energy of all channels.
+        # Energy is the power summed over time with units Joules.
+        totalEnergy = self.pdp.getTotalEnergy()
+        wpilib.SmartDashboard.putNumber("Total Energy", totalEnergy)

robotpyExamples/CONTRIBUTING.md

@@ -0,0 +1,174 @@
+Guidelines for porting WPILib examples to Python
+================================================
+
+To ensure that our examples are helpful and accurate for those learning how to
+use RobotPy, we have a set of guidelines for adding new examples to the project.
+These guidelines are not strictly enforced, but we do ask that you follow them
+when submitting pull requests with new examples. This will make the review
+process easier for everyone involved.
+
+In general, our examples are based on the Java examples from allwpilib, as Java
+is often easier to translate to Python. However, not all of our existing
+examples adhere to all of these guidelines. If you see an opportunity to improve
+an existing example, feel free to make the necessary changes.
+
+Shorter thoughts
+----------------
+
+Testing:
+
+* New examples must run! You *must* test your code on either a robot or in
+  simulation. If there's something broken in RobotPy, file an issue to get it
+  fixed
+  * You can find instructions on how to test a vision file [here](https://robotpy.readthedocs.io/en/stable/vision/other.html#vision-other-runcustom)!
+* Format your code with black
+
+General:
+
+* We always try to stay as close to the original examples as possible
+* `Main.java` is never needed
+* Don't ever check in files for your IDE (.vscode, .idea, etc)
+* Copy over the copyright statement from the original file
+
+Naming:
+
+* Filenames should always be all lowercase
+* Function names are camelCase
+* Class names start with a capital letter
+* Class method names are camelCase
+* Class member variables such as `m_name` should be `self.name` in Python
+* Protected/private methods/members can optionally be prefixed with `_`
+
+Misc conversion thoughts
+
+* Comparisons to null such as `foo == null` become `foo is None`
+* Single-line lambdas can be converted to python lambda statements. Anything
+  longer needs to be a separate function somewhere
+* Never modify `sys.path` directly!
+
+Longer thoughts
+---------------
+
+Never initialize anything other than constants at class/global scope. Here are
+a few examples:
+
+```python
+
+# OK: just a constant
+MY_CONSTANT = 42
+
+# BAD: at global scope
+motor = wpilib.Talon(1)
+
+class MyRobot:
+    # BAD: at class scope
+    motor = wpilib.Talon(1)
+
+    def __init__(self):
+        # OK: variable assigned to `self`
+        self.motor = wpilib.Talon(1)
+```
+
+---
+
+Import order doesn't really matter, but we prefer the following convention:
+
+```python
+
+# Import things from the python standard library first
+import os
+import typing
+
+# Import things from robotpy in a second group
+import wpilib
+import commands2
+
+# Import things from the other files in the example last
+import constants
+import subsystems.drivetrain
+
+```
+
+---
+
+The `pass` statement is only required for empty functions:
+
+
+```python
+# OK
+def empty_function():
+    pass
+
+def has_docstring():
+    """Some docstring"""
+    pass # NOT NEEDED
+
+class C:
+    def __init__(self):
+        super().__init__()
+        pass # NOT NEEDED
+```
+
+
+Include all the comments
+------------------------
+
+**IMPORTANT**: Include all the comments from the existing examples. These
+comments provide helpful explanations and context for the code.
+
+Converting Java comments to Python docstrings can be tedious and error prone. We
+have a tool at https://github.com/robotpy/devtools/blob/main/sphinxify_server.py
+that launches an HTML page that you can just paste doxygen or javadoc comments
+into and it will convert it to a mostly usable docstring.
+
+```python
+# 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.
+
+"""
+Some docstring describing what this file does
+"""
+
+class SomeClass:
+    """
+    This describes what this class does
+    """
+
+    def __init__(self):
+        """
+        This describes what the constructor does
+        """ 
+
+    def myFunction(self, a: int) -> int:
+        """
+        This function is great.
+
+        :param a: Input parameter a
+        """
+
+```
+
+Command-based robot specific things
+-----------------------------------
+
+We use `commands2.TimedCommandRobot` instead of TimedRobot. It provides a 
+`robotPeriodic` method for you, so it doesn't need to be included from
+the java code unless robotPeriodic function does something other than
+run the command scheduler.
+
+Java examples will often have a `Constants.java` file with a bunch of constants
+in it. RobotPy examples will put those constants in a `constants.py` as globals.
+To group constants sometimes it makes sense to put each group in its own class,
+but a single `Constants` class should be avoided.
+
+Final thoughts
+--------------
+
+Before translating WPILib Java code to RobotPy's WPILib, first take some time
+and read through the existing RobotPy code to get a feel for the style of the
+code. Try to keep it Pythonic and yet true to the original spirit of the code.
+Style *does* matter, as students will be reading through this code and it will
+potentially influence their decisions in the future.
+
+Remember, all contributions are welcome, no matter how big or small!

robotpyExamples/DifferentialDriveBot/drivetrain.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 math
+
+import wpilib
+import wpimath
+
+
+class Drivetrain:
+    """Represents a differential drive style drivetrain."""
+
+    kMaxSpeed = 3.0  # meters per second
+    kMaxAngularSpeed = 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.kFlat)
+
+        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 setSpeeds(self, speeds: wpimath.DifferentialDriveWheelSpeeds) -> None:
+        """Sets the desired wheel speeds.
+
+        :param speeds: The desired wheel speeds.
+        """
+        leftFeedforward = self.feedforward.calculate(speeds.left)
+        rightFeedforward = self.feedforward.calculate(speeds.right)
+
+        leftOutput = self.leftPIDController.calculate(
+            self.leftEncoder.getRate(), speeds.left
+        )
+        rightOutput = self.rightPIDController.calculate(
+            self.rightEncoder.getRate(), speeds.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, xSpeed: float, rot: float) -> None:
+        """Drives the robot with the given linear velocity and angular velocity.
+
+        :param xSpeed: Linear velocity in m/s.
+        :param rot: Angular velocity in rad/s.
+        """
+        wheelSpeeds = self.kinematics.toWheelSpeeds(
+            wpimath.ChassisSpeeds(xSpeed, 0.0, rot)
+        )
+        self.setSpeeds(wheelSpeeds)
+
+    def updateOdometry(self) -> None:
+        """Updates the field-relative position."""
+        self.odometry.update(
+            self.imu.getRotation2d(),
+            self.leftEncoder.getDistance(),
+            self.rightEncoder.getDistance(),
+        )

robotpyExamples/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.speedLimiter = wpimath.SlewRateLimiter(3)
+        self.rotLimiter = wpimath.SlewRateLimiter(3)
+
+    def autonomousPeriodic(self) -> None:
+        self.teleopPeriodic()
+        self.drive.updateOdometry()
+
+    def teleopPeriodic(self) -> None:
+        # Get the x speed. We are inverting this because Xbox controllers return
+        # negative values when we push forward.
+        xSpeed = (
+            -self.speedLimiter.calculate(self.controller.getLeftY())
+            * Drivetrain.kMaxSpeed
+        )
+
+        # 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.kMaxAngularSpeed
+        )
+
+        self.drive.drive(xSpeed, rot)

robotpyExamples/DifferentialDrivePoseEstimator/drivetrain.py

@@ -0,0 +1,260 @@
+#
+# 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."""
+
+    kMaxSpeed = 3.0  # meters per second
+    kMaxAngularSpeed = 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.kFlat)
+
+        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 setSpeeds(self, speeds: wpimath.DifferentialDriveWheelSpeeds) -> None:
+        """Sets the desired wheel speeds.
+
+        :param speeds: The desired wheel speeds.
+        """
+        leftFeedforward = self.feedforward.calculate(speeds.left)
+        rightFeedforward = self.feedforward.calculate(speeds.right)
+
+        leftOutput = self.leftPIDController.calculate(
+            self.leftEncoder.getRate(), speeds.left
+        )
+        rightOutput = self.rightPIDController.calculate(
+            self.rightEncoder.getRate(), speeds.right
+        )
+        self.leftLeader.setVoltage(leftOutput + leftFeedforward)
+        self.rightLeader.setVoltage(rightOutput + rightFeedforward)
+
+    def drive(self, xSpeed: float, rot: float) -> None:
+        """Drives the robot with the given linear velocity and angular velocity.
+
+        :param xSpeed: Linear velocity in m/s.
+        :param rot: Angular velocity in rad/s.
+        """
+        wheelSpeeds = self.kinematics.toWheelSpeeds(
+            wpimath.ChassisSpeeds(xSpeed, 0.0, rot)
+        )
+        self.setSpeeds(wheelSpeeds)
+
+    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.get() * wpilib.RobotController.getInputVoltage(),
+            self.rightLeader.get() * 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/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/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("m_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.speedLimiter = 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 speed. We are inverting this because Xbox controllers return
+        # negative values when we push forward.
+        xSpeed = -self.speedLimiter.calculate(self.controller.getLeftY())
+        xSpeed *= Drivetrain.kMaxSpeed
+
+        # 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.kMaxAngularSpeed
+
+        self.drive.drive(xSpeed, rot)

robotpyExamples/DigitalCommunication/robot.py

@@ -0,0 +1,49 @@
+#!/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 demonstrating how to communicate to a light controller from the robot
+    code using the roboRIO's DIO ports.
+    """
+
+    # define ports for digitalcommunication with light controller
+    kAlliancePort = 0
+    kEnabledPort = 1
+    kAutonomousPort = 2
+    kAlertPort = 3
+
+    def __init__(self):
+        """Robot initialization function"""
+        super().__init__()
+
+        self.allianceOutput = wpilib.DigitalOutput(self.kAlliancePort)
+        self.enabledOutput = wpilib.DigitalOutput(self.kEnabledPort)
+        self.autonomousOutput = wpilib.DigitalOutput(self.kAutonomousPort)
+        self.alertOutput = wpilib.DigitalOutput(self.kAlertPort)
+
+    def robotPeriodic(self):
+        setAlliance = False
+        alliance = wpilib.DriverStation.getAlliance()
+        if alliance:
+            setAlliance = alliance == wpilib.DriverStation.Alliance.kRed
+
+        # pull alliance port high if on red alliance, pull low if on blue alliance
+        self.allianceOutput.set(setAlliance)
+
+        # pull enabled port high if enabled, low if disabled
+        self.enabledOutput.set(wpilib.DriverStation.isEnabled())
+
+        # pull auto port high if in autonomous, low if in teleop (or disabled)
+        self.autonomousOutput.set(wpilib.DriverStation.isAutonomous())
+
+        # pull alert port high if match time remaining is between 30 and 25 seconds
+        matchTime = wpilib.DriverStation.getMatchTime()
+        self.alertOutput.set(30 >= matchTime >= 25)

robotpyExamples/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
+
+    kMaxSpeed = 3.0  # m/s
+    kMaxAcceleration = 3.0  # m/s²
+
+
+class OIConstants:
+    kDriverControllerPort = 0

robotpyExamples/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(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._speed = 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, speed: float) -> None:
+        self._speed = -speed if self._inverted else speed
+
+    def get(self) -> float:
+        return self._speed
+
+    def setInverted(self, isInverted: bool) -> None:
+        self._inverted = isInverted
+
+    def getInverted(self) -> bool:
+        return self._inverted
+
+    def disable(self) -> None:
+        self._speed = 0.0
+
+    def stopMotor(self) -> None:
+        self._speed = 0.0

robotpyExamples/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 testInit(self) -> None:
+        # Cancels all running commands at the start of test mode
+        commands2.CommandScheduler.getInstance().cancelAll()

robotpyExamples/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.driveFullSpeed = commands2.cmd.runOnce(
+            lambda: self.robotDrive.setMaxOutput(1), self.robotDrive
+        )
+        self.driveHalfSpeed = 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 speed when the bumper is held
+        self.driverController.rightBumper().onTrue(self.driveHalfSpeed).onFalse(
+            self.driveFullSpeed
+        )
+
+        # 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/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, self.rightLeader)
+
+        self.profile = wpimath.TrapezoidProfile(
+            wpimath.TrapezoidProfile.Constraints(
+                constants.DriveConstants.kMaxSpeed,
+                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/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/DutyCycleInput/robot.py

@@ -0,0 +1,27 @@
+#!/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):
+        """Robot initialization function"""
+        super().__init__()
+
+        self.dutyCycle = wpilib.DutyCycle(0)
+
+    def robotPeriodic(self):
+        # Duty Cycle Frequency in Hz
+        frequency = self.dutyCycle.getFrequency()
+
+        # Output of duty cycle, ranging from 0 to 1
+        # 1 is fully on, 0 is fully off
+        output = self.dutyCycle.getOutput()
+
+        wpilib.SmartDashboard.putNumber("Frequency", frequency)
+        wpilib.SmartDashboard.putNumber("Duty Cycle", output)

robotpyExamples/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, speed: 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/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/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/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/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.getSpeed() * 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.set(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/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/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/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/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.getSpeed() * 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.set(0.0)
+
+    def updateTelemetry(self) -> None:
+        """Update telemetry, including the mechanism visualization."""
+        # Update elevator visualization with position
+        self.elevatorMech2d.setLength(self.encoder.getDistance())

robotpyExamples/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._speed = 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, speed: float) -> None:
+        self._speed = -speed if self._inverted else speed
+
+    def get(self) -> float:
+        return self._speed
+
+    def setInverted(self, isInverted: bool) -> None:
+        self._inverted = isInverted
+
+    def getInverted(self) -> bool:
+        return self._inverted
+
+    def disable(self) -> None:
+        self._speed = 0.0
+
+    def stopMotor(self) -> None:
+        self._speed = 0.0

robotpyExamples/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/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.k4X)
+
+        # 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/EventLoop/robot.py

@@ -0,0 +1,87 @@
+#!/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
+
+SHOT_VELOCITY = 200.0  # rpm
+TOLERANCE = 8.0  # rpm
+
+
+class MyRobot(wpilib.TimedRobot):
+    def __init__(self) -> None:
+        super().__init__()
+
+        self.shooter = wpilib.PWMSparkMax(0)
+        self.shooterEncoder = wpilib.Encoder(0, 1)
+        self.controller = wpimath.PIDController(0.3, 0, 0)
+        self.feedforward = wpimath.SimpleMotorFeedforwardMeters(0.1, 0.065)
+
+        self.kicker = wpilib.PWMSparkMax(1)
+
+        self.intake = wpilib.PWMSparkMax(2)
+
+        self.loop = wpilib.EventLoop()
+        self.joystick = wpilib.Joystick(0)
+
+        # Called once at the beginning of the robot program.
+        self.controller.setTolerance(TOLERANCE)
+
+        isBallAtKickerEvent = wpilib.BooleanEvent(
+            self.loop, lambda: False
+        )  # self.kickerSensor.getRange() < KICKER_THRESHOLD
+        intakeButton = wpilib.BooleanEvent(
+            self.loop, lambda: self.joystick.getRawButton(2)
+        )
+
+        # if the thumb button is held
+        intakeButton.and_(isBallAtKickerEvent.negate()).ifHigh(
+            # and there is not a ball at the kicker
+            # activate the intake
+            lambda: self.intake.set(0.5)
+        )
+
+        # if the thumb button is not held
+        intakeButton.negate().or_(isBallAtKickerEvent).ifHigh(
+            # or there is a ball in the kicker
+            # stop the intake
+            self.intake.stopMotor
+        )
+
+        shootTrigger = wpilib.BooleanEvent(self.loop, self.joystick.getTrigger)
+
+        # if the trigger is held
+        shootTrigger.ifHigh(
+            # accelerate the shooter wheel
+            lambda: self.shooter.setVoltage(
+                self.controller.calculate(self.shooterEncoder.getRate(), SHOT_VELOCITY)
+                + self.feedforward.calculate(SHOT_VELOCITY)
+            )
+        )
+
+        # if not, stop
+        shootTrigger.negate().ifHigh(self.shooter.stopMotor)
+
+        atTargetVelocity = wpilib.BooleanEvent(
+            self.loop, self.controller.atSetpoint
+        ).debounce(
+            # debounce for more stability
+            0.2
+        )
+
+        # if we're at the target velocity, kick the ball into the shooter wheel
+        atTargetVelocity.ifHigh(lambda: self.kicker.set(0.7))
+
+        # when we stop being at the target velocity, it means the ball was shot
+        atTargetVelocity.falling().ifHigh(
+            # so stop the kicker
+            self.kicker.stopMotor
+        )
+
+    def robotPeriodic(self) -> None:
+        # poll all the bindings
+        self.loop.poll()

robotpyExamples/FlywheelBangBangController/robot.py

@@ -0,0 +1,108 @@
+#!/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
+
+import wpilib.simulation
+import wpimath
+import wpimath.units
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    This is a sample program to demonstrate the use of a BangBangController with a flywheel to
+    control RPM.
+    """
+
+    kMotorPort = 0
+    kEncoderAChannel = 0
+    kEncoderBChannel = 1
+
+    # Max setpoint for joystick control in RPM
+    kMaxSetpointValue = 6000.0
+
+    # Gains are for example purposes only - must be determined for your own robot!
+    kFlywheelKs = 0.0001  # V
+    kFlywheelKv = 0.000195  # V/RPM
+    kFlywheelKa = 0.0003  # V/(RPM/s)
+
+    # 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
+
+    # 1/2 MR²
+    kFlywheelMomentOfInertia = (
+        0.5
+        * wpimath.units.lbsToKilograms(1.5)
+        * math.pow(wpimath.units.inchesToMeters(4), 2)
+    )
+
+    def __init__(self) -> None:
+        """Robot initialization function"""
+        super().__init__()
+
+        self.flywheelMotor = wpilib.PWMSparkMax(self.kMotorPort)
+        self.encoder = wpilib.Encoder(self.kEncoderAChannel, self.kEncoderBChannel)
+
+        self.bangBangControler = wpimath.BangBangController()
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.feedforward = wpimath.SimpleMotorFeedforwardMeters(
+            self.kFlywheelKs, self.kFlywheelKv, self.kFlywheelKa
+        )
+
+        # Joystick to control setpoint
+        self.joystick = wpilib.Joystick(0)
+
+        # Simulation classes help us simulate our robot
+
+        self.gearbox = wpimath.DCMotor.NEO(1)
+
+        self.plant = wpimath.Models.flywheelFromPhysicalConstants(
+            self.gearbox, self.kFlywheelGearing, self.kFlywheelMomentOfInertia
+        )
+
+        self.flywheelSim = wpilib.simulation.FlywheelSim(self.plant, self.gearbox)
+        self.encoderSim = wpilib.simulation.EncoderSim(self.encoder)
+
+        # Add bang-bang controller to SmartDashboard and networktables.
+        wpilib.SmartDashboard.putData(self.bangBangControler)
+
+    def teleopPeriodic(self) -> None:
+        """Controls flywheel to a set speed (RPM) controlled by a joystick."""
+
+        # Scale setpoint value between 0 and maxSetpointValue
+        setpoint = max(
+            0.0,
+            self.joystick.getRawAxis(0)
+            * wpimath.units.rotationsPerMinuteToRadiansPerSecond(
+                self.kMaxSetpointValue
+            ),
+        )
+
+        # Set setpoint and measurement of the bang-bang controller
+        bangOutput = (
+            self.bangBangControler.calculate(self.encoder.getRate(), setpoint) * 12.0
+        )
+
+        # Controls a motor with the output of the BangBang controller and a
+        # feedforward. The feedforward is reduced slightly to avoid overspeeding
+        # the shooter.
+        self.flywheelMotor.setVoltage(
+            bangOutput + 0.9 * self.feedforward.calculate(setpoint)
+        )
+
+    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 velocities to our simulated encoder
+        self.flywheelSim.setInputVoltage(
+            self.flywheelMotor.get() * wpilib.RobotController.getInputVoltage()
+        )
+        self.flywheelSim.update(0.02)
+        self.encoderSim.setRate(self.flywheelSim.getAngularVelocity())

robotpyExamples/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 speed, 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 testInit(self):
+        """This function is called once each time the robot enters test mode."""
+
+    def testPeriodic(self):
+        """This function is called periodically during test mode."""

robotpyExamples/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.kFlat
+    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 speed 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/GyroMecanum/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.kFlat
+    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/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.kAutoDriveSpeed, 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.kAutoDriveSpeed, 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.kAutoDriveSpeed, 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/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.CTREPCM
+kHatchSolenoidModule = 0
+kHatchSolenoidPorts = (0, 1)
+
+# Autonomous
+kAutoDriveDistanceInches = 60
+kAutoBackupDistanceInches = 20
+kAutoDriveSpeed = 0.5
+
+# Operator Interface
+kDriverControllerPort = 0
+
+# Physical parameters
+kDriveTrainMotorCount = 2
+kTrackWidth = 0.381 * 2
+kGearingRatio = 8
+kWheelRadius = 0.0508
+
+# kEncoderResolution = -

robotpyExamples/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 testInit(self) -> None:
+        # Cancels all running commands at the start of test mode
+        commands2.CommandScheduler.getInstance().cancelAll()

robotpyExamples/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 speed
+        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/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 speeds
+        # 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/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.kForward), self
+        )
+
+    def releaseHatch(self) -> commands2.Command:
+        """Releases the hatch"""
+        return commands2.cmd.runOnce(
+            lambda: self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse), self
+        )

robotpyExamples/HatchbotTraditional/commands/complexauto.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.
+#
+
+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.kAutoDriveSpeed, drive
+            ),
+            # Release the hatch
+            ReleaseHatch(hatch),
+            # Drive backward the specified distance
+            DriveDistance(
+                constants.kAutoBackupDistanceInches, -constants.kAutoDriveSpeed, drive
+            ),
+        )

robotpyExamples/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/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, speed: float, drive: DriveSubsystem) -> None:
+        self.distance = inches
+        self.speed = speed
+        self.drive = drive
+        self.addRequirements(drive)
+
+    def initialize(self) -> None:
+        self.drive.resetEncoders()
+        self.drive.arcadeDrive(self.speed, 0)
+
+    def execute(self) -> None:
+        self.drive.arcadeDrive(self.speed, 0)
+
+    def end(self, interrupted: bool) -> None:
+        self.drive.arcadeDrive(0, 0)
+
+    def isFinished(self) -> bool:
+        return abs(self.drive.getAverageEncoderDistance()) >= self.distance

robotpyExamples/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/HatchbotTraditional/commands/halvedrivespeed.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 HalveDriveSpeed(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/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/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.CTREPCM
+kHatchSolenoidModule = 0
+kHatchSolenoidPorts = (0, 1)
+
+# Autonomous
+kAutoDriveDistanceInches = 60
+kAutoBackupDistanceInches = 20
+kAutoDriveSpeed = 0.5
+
+# Operator Interface
+kDriverControllerPort = 0
+
+# Physical parameters
+kDriveTrainMotorCount = 2
+kTrackWidth = 0.381 * 2
+kGearingRatio = 8
+kWheelRadius = 0.0508
+
+# kEncoderResolution = -

robotpyExamples/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 testInit(self) -> None:
+        # Cancels all running commands at the start of test mode
+        commands2.CommandScheduler.getInstance().cancelAll()

robotpyExamples/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.halvedrivespeed import HalveDriveSpeed
+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.kAutoDriveSpeed, 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(
+            HalveDriveSpeed(self.drive)
+        )
+
+    def getAutonomousCommand(self) -> commands2.Command:
+        return self.chooser.getSelected()

robotpyExamples/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 speeds
+        # 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/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.kForward)
+
+    def releaseHatch(self) -> None:
+        """Releases the hatch"""
+        self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse)

robotpyExamples/HidRumble/robot.py

@@ -0,0 +1,30 @@
+#!/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 GenericHID's rumble feature.
+    """
+
+    def __init__(self):
+        """Robot initialization function"""
+        super().__init__()
+
+        self.hid = wpilib.NiDsXboxController(0)
+
+    def autonomousInit(self):
+        # Turn on rumble at the start of auto
+        self.hid.setRumble(wpilib.NiDsXboxController.RumbleType.kLeftRumble, 1.0)
+        self.hid.setRumble(wpilib.NiDsXboxController.RumbleType.kRightRumble, 1.0)
+
+    def disabledInit(self):
+        # Stop the rumble when entering disabled
+        self.hid.setRumble(wpilib.NiDsXboxController.RumbleType.kLeftRumble, 0.0)
+        self.hid.setRumble(wpilib.NiDsXboxController.RumbleType.kRightRumble, 0.0)

robotpyExamples/HttpCamera/robot.py

@@ -0,0 +1,20 @@
+#!/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 wpilib.cameraserver
+
+
+class MyRobot(wpilib.TimedRobot):
+    def __init__(self):
+        super().__init__()
+        # Your image processing code will be launched via a stub that will set up logging and initialize NetworkTables
+        # to talk to your robot code.
+        # https://robotpy.readthedocs.io/en/stable/vision/roborio.html#important-notes
+
+        wpilib.CameraServer.launch("vision.py:main")

robotpyExamples/HttpCamera/vision.py

@@ -0,0 +1,71 @@
+#
+# 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 is a demo program showing the use of OpenCV to do vision processing. The image is acquired
+from an HTTP camera, then a rectangle is put on the image and sent to the dashboard. OpenCV has
+many methods for different types of processing.
+"""
+
+import numpy
+import cscore
+from cscore import CameraServer
+import cv2
+
+#
+# This code will work both on a RoboRIO and on other platforms. The exact mechanism
+# to run it differs depending on whether you’re on a RoboRIO or a coprocessor
+#
+# https://robotpy.readthedocs.io/en/stable/vision/code.html
+
+
+def main():
+    # Create an HTTP camera. The address will need to be modified to have the
+    # correct team number. The exact path will depend on the source.
+    camera = cscore.HttpCamera("HTTP Camera", "http://10.x.y.11/video/stream.mjpg")
+    # Start capturing images
+    CameraServer.startAutomaticCapture(camera)
+    # Set the resolution
+    camera.setResolution(640, 480)
+
+    # Get a CvSink. This will capture Mats from the camera
+    cvSink = CameraServer.getVideo()
+
+    # Setup a CvSource. This will send images back to the Dashboard
+    outputStream = CameraServer.putVideo("Rectangle", 640, 480)
+
+    # Mats are very memory expensive. Lets reuse this Mat.
+    mat = numpy.zeros((480, 640, 3), dtype="uint8")
+
+    # Declare the color of the rectangle
+    rectColor = (255, 255, 255)
+
+    # The camera code will be killed when the robot.py program exits. If you wish to perform cleanup,
+    # you should register an atexit handler. The child process will NOT be launched when running the robot code in
+    # simulation or unit testing mode
+
+    while True:
+        # Tell the CvSink to grab a frame from the camera and put it in the source mat.  If there is an error notify the
+        # output.
+
+        if cvSink.grabFrame(mat) == 0:
+            # Send the output the error.
+            outputStream.notifyError(cvSink.getError())
+
+            # skip the rest of the current iteration
+            continue
+
+        # Put a rectangle on the image
+        mat = cv2.rectangle(
+            img=mat,
+            pt1=(100, 100),
+            pt2=(400, 400),
+            color=rectColor,
+            lineType=5,
+        )
+
+        # Give the output stream a new image to display
+        outputStream.putFrame(mat)

robotpyExamples/I2CCommunication/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):
+    """
+    This is a sample program demonstrating how to communicate to a light controller from the robot
+    code using the roboRIO's I2C port.
+    """
+
+    PORT = wpilib.I2C.Port.kPort0
+    DEVICE_ADDRESS = 4
+
+    def __init__(self):
+        """Robot initialization function"""
+        super().__init__()
+
+        self.arduino = wpilib.I2C(self.PORT, self.DEVICE_ADDRESS)
+
+    def writeString(self, s: str):
+        # Creates a char array from the input string
+        chars = s.encode("ascii")
+
+        # Writes bytes over I2C
+        self.arduino.writeBulk(chars)
+
+    def robotPeriodic(self):
+        # Creates a string to hold current robot state information, including
+        # alliance, enabled state, operation mode, and match time. The message
+        # is sent in format "AEM###" where A is the alliance color, (R)ed or
+        # (B)lue, E is the enabled state, (E)nabled or (D)isabled, M is the
+        # operation mode, (A)utonomous or (T)eleop, and ### is the zero-padded
+        # time remaining in the match.
+        #
+        # For example, "RET043" would indicate that the robot is on the red
+        # alliance, enabled in teleop mode, with 43 seconds left in the match.
+        allianceString = "U"
+        alliance = wpilib.DriverStation.getAlliance()
+        if alliance is not None:
+            allianceString = (
+                "R" if alliance == wpilib.DriverStation.Alliance.kRed else "B"
+            )
+
+        enabledString = "E" if wpilib.DriverStation.isEnabled() else "D"
+        autoString = "A" if wpilib.DriverStation.isAutonomous() else "T"
+        matchTime = wpilib.DriverStation.getMatchTime()
+
+        stateMessage = f"{allianceString}{enabledString}{autoString}{matchTime:03f}"
+
+        self.writeString(stateMessage)

robotpyExamples/IntermediateVision/robot.py

@@ -0,0 +1,20 @@
+#!/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 OpenCV to do vision processing. The image is acquired
+    from the USB camera, then a rectangle is put on the image and sent to the dashboard. OpenCV has
+    many methods for different types of processing.
+    """
+
+    def __init__(self):
+        super().__init__()
+        wpilib.CameraServer.launch("vision.py:main")

robotpyExamples/IntermediateVision/vision.py

@@ -0,0 +1,53 @@
+#
+# 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 is a demo program showing the use of OpenCV to do vision processing. The image is acquired
+# from the USB camera, then a rectangle is put on the image and sent to the dashboard. OpenCV has
+# many methods for different types of processing.
+#
+# NOTE: This code runs in its own process, so we cannot access the robot here,
+#       nor can we create/use/see wpilib objects
+#
+# To try this code out locally (if you have robotpy-cscore installed), you
+# can execute `python3 -m cscore vision.py:main`
+#
+
+import cv2
+import numpy as np
+
+from cscore import CameraServer as CS
+
+
+def main():
+    # Get the UsbCamera from CameraServer
+    camera = CS.startAutomaticCapture()
+    # Set the resolution
+    camera.setResolution(640, 480)
+
+    # Get a CvSink. This will capture images from the camera
+    cvSink = CS.getVideo()
+    # Setup a CvSource. This will send images back to the Dashboard
+    outputStream = CS.putVideo("Rectangle", 640, 480)
+
+    # Mats are very memory expensive. Lets reuse this Mat.
+    mat = np.zeros(shape=(480, 640, 3), dtype=np.uint8)
+
+    while True:
+        # Tell the CvSink to grab a frame from the camera and put it
+        # in the source image.  If there is an error notify the output.
+        time, mat = cvSink.grabFrame(mat)
+        if time == 0:
+            # Send the output the error.
+            outputStream.notifyError(cvSink.getError())
+            # skip the rest of the current iteration
+            continue
+
+        # Put a rectangle on the image
+        cv2.rectangle(mat, (100, 100), (400, 400), (255, 255, 255), 5)
+
+        # Give the output stream a new image to display
+        outputStream.putFrame(mat)

robotpyExamples/MecanumBot/drivetrain.py

@@ -0,0 +1,127 @@
+#
+# 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."""
+
+    kMaxSpeed = 3.0  # 3 meters per second
+    kMaxAngularSpeed = 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.kFlat)
+
+        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.MecanumDriveWheelSpeeds:
+        """Returns the current state of the drivetrain."""
+        return wpimath.MecanumDriveWheelSpeeds(
+            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 setSpeeds(self, speeds: wpimath.MecanumDriveWheelSpeeds) -> None:
+        """Sets the desired speeds for each wheel."""
+        frontLeftFeedforward = self.feedforward.calculate(speeds.frontLeft)
+        frontRightFeedforward = self.feedforward.calculate(speeds.frontRight)
+        backLeftFeedforward = self.feedforward.calculate(speeds.rearLeft)
+        backRightFeedforward = self.feedforward.calculate(speeds.rearRight)
+
+        frontLeftOutput = self.frontLeftPIDController.calculate(
+            self.frontLeftEncoder.getRate(), speeds.frontLeft
+        )
+        frontRightOutput = self.frontRightPIDController.calculate(
+            self.frontRightEncoder.getRate(), speeds.frontRight
+        )
+        backLeftOutput = self.frontLeftPIDController.calculate(
+            self.backLeftEncoder.getRate(), speeds.rearLeft
+        )
+        backRightOutput = self.frontRightPIDController.calculate(
+            self.backRightEncoder.getRate(), speeds.rearRight
+        )
+
+        self.frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward)
+        self.frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward)
+        self.backLeftMotor.setVoltage(backLeftOutput + backLeftFeedforward)
+        self.backRightMotor.setVoltage(backRightOutput + backRightFeedforward)
+
+    def drive(
+        self,
+        xSpeed: float,
+        ySpeed: float,
+        rot: float,
+        fieldRelative: bool,
+        periodSeconds: float,
+    ) -> None:
+        """Method to drive the robot using joystick info."""
+        chassisSpeeds = wpimath.ChassisSpeeds(xSpeed, ySpeed, rot)
+        if fieldRelative:
+            chassisSpeeds = chassisSpeeds.toRobotRelative(self.imu.getRotation2d())
+
+        self.setSpeeds(
+            self.kinematics.toWheelSpeeds(
+                chassisSpeeds.discretize(periodSeconds)
+            ).desaturate(self.kMaxSpeed)
+        )
+
+    def updateOdometry(self) -> None:
+        """Updates the field-relative position."""
+        self.odometry.update(self.imu.getRotation2d(), self.getCurrentDistances())

robotpyExamples/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.xspeedLimiter = wpimath.SlewRateLimiter(3)
+        self.yspeedLimiter = 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 speed. We are inverting this because Xbox controllers return
+        # negative values when we push forward.
+        xSpeed = (
+            -self.xspeedLimiter.calculate(self.controller.getLeftY())
+            * Drivetrain.kMaxSpeed
+        )
+
+        # Get the y speed or sideways/strafe speed. 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.
+        ySpeed = (
+            -self.yspeedLimiter.calculate(self.controller.getLeftX())
+            * Drivetrain.kMaxSpeed
+        )
+
+        # 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.kMaxAngularSpeed
+        )
+
+        self.mecanum.drive(xSpeed, ySpeed, rot, fieldRelative, self.getPeriod())

robotpyExamples/MecanumDrive/robot.py

@@ -0,0 +1,51 @@
+#!/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 how to use Mecanum control with the
+    MecanumDrive class.
+    """
+
+    # Channels on the roboRIO that the motor controllers are plugged in to
+    kFrontLeftChannel = 2
+    kRearLeftChannel = 3
+    kFrontRightChannel = 1
+    kRearRightChannel = 0
+
+    # The channel on the driver station that the joystick is connected to
+    kJoystickChannel = 0
+
+    def __init__(self):
+        super().__init__()
+        self.frontLeft = wpilib.PWMSparkMax(self.kFrontLeftChannel)
+        self.rearLeft = wpilib.PWMSparkMax(self.kRearLeftChannel)
+        self.frontRight = wpilib.PWMSparkMax(self.kFrontRightChannel)
+        self.rearRight = wpilib.PWMSparkMax(self.kRearRightChannel)
+
+        # invert the right side motors
+        # you may need to change or remove this to match your robot
+        self.frontRight.setInverted(True)
+        self.rearRight.setInverted(True)
+
+        self.robotDrive = wpilib.MecanumDrive(
+            self.frontLeft, self.rearLeft, self.frontRight, self.rearRight
+        )
+
+        self.stick = wpilib.Joystick(self.kJoystickChannel)
+
+    def teleopPeriodic(self):
+        # Use the joystick X axis for lateral movement, Y axis for forward
+        # movement, and Z axis for rotation.
+        self.robotDrive.driveCartesian(
+            -self.stick.getY(),
+            -self.stick.getX(),
+            -self.stick.getZ(),
+        )

robotpyExamples/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."""
+
+    kMaxSpeed = 3.0  # 3 meters per second
+    kMaxAngularSpeed = 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.kFlat)
+
+        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.MecanumDriveWheelSpeeds:
+        """Returns the current state of the drivetrain.
+
+        :returns: The current state of the drivetrain.
+        """
+        return wpimath.MecanumDriveWheelSpeeds(
+            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 setSpeeds(self, speeds: wpimath.MecanumDriveWheelSpeeds) -> None:
+        """Set the desired speeds for each wheel.
+
+        :param speeds: The desired wheel speeds.
+        """
+        frontLeftFeedforward = self.feedforward.calculate(speeds.frontLeft)
+        frontRightFeedforward = self.feedforward.calculate(speeds.frontRight)
+        backLeftFeedforward = self.feedforward.calculate(speeds.rearLeft)
+        backRightFeedforward = self.feedforward.calculate(speeds.rearRight)
+
+        frontLeftOutput = self.frontLeftPIDController.calculate(
+            self.frontLeftEncoder.getRate(), speeds.frontLeft
+        )
+        frontRightOutput = self.frontRightPIDController.calculate(
+            self.frontRightEncoder.getRate(), speeds.frontRight
+        )
+        backLeftOutput = self.backLeftPIDController.calculate(
+            self.backLeftEncoder.getRate(), speeds.rearLeft
+        )
+        backRightOutput = self.backRightPIDController.calculate(
+            self.backRightEncoder.getRate(), speeds.rearRight
+        )
+
+        self.frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward)
+        self.frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward)
+        self.backLeftMotor.setVoltage(backLeftOutput + backLeftFeedforward)
+        self.backRightMotor.setVoltage(backRightOutput + backRightFeedforward)
+
+    def drive(
+        self,
+        xSpeed: float,
+        ySpeed: float,
+        rot: float,
+        fieldRelative: bool,
+        period: float,
+    ) -> None:
+        """Method to drive the robot using joystick info.
+
+        :param xSpeed: Speed of the robot in the x direction (forward).
+        :param ySpeed: Speed of the robot in the y direction (sideways).
+        :param rot: Angular rate of the robot.
+        :param fieldRelative: Whether the provided x and y speeds are relative to the field.
+        """
+        chassisSpeeds = wpimath.ChassisSpeeds(xSpeed, ySpeed, rot)
+        if fieldRelative:
+            chassisSpeeds = chassisSpeeds.toRobotRelative(
+                self.poseEstimator.getEstimatedPosition().rotation()
+            )
+        self.setSpeeds(
+            self.kinematics.toWheelSpeeds(chassisSpeeds.discretize(period)).desaturate(
+                self.kMaxSpeed
+            )
+        )
+
+    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/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/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.xspeedLimiter = wpimath.SlewRateLimiter(3)
+        self.yspeedLimiter = 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 speed. We are inverting this because Xbox controllers return
+        # negative values when we push forward.
+        xSpeed = -self.xspeedLimiter.calculate(self.controller.getLeftY())
+        xSpeed *= Drivetrain.kMaxSpeed
+
+        # Get the y speed or sideways/strafe speed. 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.
+        ySpeed = -self.yspeedLimiter.calculate(self.controller.getLeftX())
+        ySpeed *= Drivetrain.kMaxSpeed
+
+        # 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.kMaxAngularSpeed
+
+        self.mecanum.drive(xSpeed, ySpeed, rot, fieldRelative, self.getPeriod())

robotpyExamples/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.kPurple)
+        )
+
+        # 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.set(self.joystick.getRawAxis(0))
+        self.wristMotor.set(self.joystick.getRawAxis(1))

robotpyExamples/MotorControl/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 wpilib
+
+import math
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    This sample program shows how to control a motor using a joystick. In the operator control part
+    of the program, the joystick is read and the value is written to the motor.
+
+    Joystick analog values range from -1 to 1 and motor controller inputs also range from -1 to 1
+    making it easy to work together.
+
+    In addition, the encoder value of an encoder connected to ports 0 and 1 is consistently sent
+    to the Dashboard.
+    """
+
+    kMotorPort = 0
+    kJoystickPort = 0
+    kEncoderPortA = 0
+    kEncoderPortB = 1
+
+    def __init__(self):
+        """Robot initialization function"""
+        super().__init__()
+
+        self.motor = wpilib.PWMSparkMax(self.kMotorPort)
+        self.joystick = wpilib.Joystick(self.kJoystickPort)
+        self.encoder = wpilib.Encoder(self.kEncoderPortA, self.kEncoderPortB)
+        # Use SetDistancePerPulse to set the multiplier for GetDistance
+        # This is set up assuming a 6 inch wheel with a 360 CPR encoder.
+        self.encoder.setDistancePerPulse((math.pi * 6) / 360.0)
+
+    def robotPeriodic(self):
+        """The RobotPeriodic function is called every control packet no matter the robot mode."""
+        wpilib.SmartDashboard.putNumber("Encoder", self.encoder.getDistance())
+
+    def teleopPeriodic(self):
+        self.motor.set(self.joystick.getY())

robotpyExamples/PotentiometerPID/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.
+#
+
+import wpilib
+import wpimath
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    This is a sample program to demonstrate how to use a soft potentiometer and a PID controller to
+    reach and maintain position setpoints on an elevator mechanism.
+    """
+
+    kPotChannel = 1
+    kMotorChannel = 7
+    kJoystickChannel = 3
+
+    # The elevator can move 1.5 meters from top to bottom
+    kFullHeightMeters = 1.5
+
+    # Bottom, middle, and top elevator setpoints
+    kSetpointMeters = [0.2, 0.8, 1.4]
+
+    # proportional, integral, and derivative speed constants
+    # DANGER: when tuning PID constants, high/inappropriate values for kP, kI,
+    # and kD may cause dangerous, uncontrollable, or undesired behavior!
+    kP = 0.7
+    kI = 0.35
+    kD = 0.25
+
+    def __init__(self):
+        super().__init__()
+
+        self.pidController = wpimath.PIDController(self.kP, self.kI, self.kD)
+        # Scaling is handled internally
+        self.potentiometer = wpilib.AnalogPotentiometer(
+            self.kPotChannel, self.kFullHeightMeters
+        )
+        self.elevatorMotor = wpilib.PWMSparkMax(self.kMotorChannel)
+        self.joystick = wpilib.Joystick(self.kJoystickChannel)
+
+    def teleopInit(self):
+        # Move to the bottom setpoint when teleop starts
+        self.index = 0
+        self.pidController.setSetpoint(self.kSetpointMeters[self.index])
+
+    def teleopPeriodic(self):
+        # Read from the sensor
+        position = self.potentiometer.get()
+
+        # Run the PID Controller
+        pidOut = self.pidController.calculate(position)
+
+        # Apply PID output
+        self.elevatorMotor.set(pidOut)
+
+        # when the button is pressed once, the selected elevator setpoint is incremented
+        if self.joystick.getTriggerPressed():
+            # index of the elevator setpoint wraps around.
+            self.index = (self.index + 1) % len(self.kSetpointMeters)
+            print(f"index = {self.index}")
+            self.pidController.setSetpoint(self.kSetpointMeters[self.index])

robotpyExamples/QuickVision/robot.py

@@ -0,0 +1,21 @@
+#!/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 wpilib.cameraserver import CameraServer
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    Uses the CameraServer class to automatically capture video from a USB webcam and send it to the
+    FRC dashboard without doing any vision processing. This is the easiest way to get camera images
+    to the dashboard. Just add this to the robotInit() method in your program.
+    """
+
+    def __init__(self):
+        super().__init__()
+        CameraServer().launch()

robotpyExamples/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 speed
+    kV = 12.0 / kShooterFreeRPS  # V/(rot/s)
+
+    kFeederSpeed = 0.5
+
+
+class IntakeConstants:
+    kMotorPort = 6
+    kSolenoidPorts = (2, 3)
+
+
+class StorageConstants:
+    kMotorPort = 7
+    kBallSensorPort = 6
+
+
+class AutoConstants:
+    kTimeout = 3
+    kDriveDistance = 2.0  # m
+    kDriveSpeed = 0.5
+
+
+class OIConstants:
+    kDriverControllerPort = 0

robotpyExamples/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 speed with a 3 second timeout
+        return self.drive.driveDistanceCommand(
+            AutoConstants.kDriveDistance, AutoConstants.kDriveSpeed
+        ).withTimeout(AutoConstants.kTimeout)

robotpyExamples/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 testInit(self) -> None:
+        # Cancels all running commands at the start of test mode.
+        commands2.CommandScheduler.getInstance().cancelAll()
+
+    def testPeriodic(self) -> None:
+        """This function is called periodically during test mode."""
+        pass

robotpyExamples/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.kFlat)
+        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, speed: float) -> Command:
+        """Returns a command that drives the robot forward a specified distance at a specified
+        speed.
+
+        :param distance: The distance to drive forward in meters
+        :param speed: The fraction of max speed at which to drive
+        """
+        return (
+            self.runOnce(
+                lambda: (
+                    self.leftEncoder.reset(),
+                    self.rightEncoder.reset(),
+                )
+            )
+            .andThen(self.run(lambda: self.drive.arcadeDrive(speed, 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/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.CTREPCM,
+            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.kForward))
+            .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.kReverse),
+            )
+        ).withName("Retract")

robotpyExamples/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.CTREPCM)
+
+    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/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/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/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 speed 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 speed
+        :param rotation:    Callable supplier of rotational speed
+        """
+
+        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/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/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/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, speed: float, inches: float, drive: Drivetrain) -> None:
+        """Creates a new DriveDistance. This command will drive your your robot for a desired distance at
+        a desired speed.
+
+        :param speed:  The speed 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.speed = speed
+        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.speed, 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/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 speed and time."""
+
+    def __init__(self, speed: float, time: float, drive: Drivetrain) -> None:
+        """Creates a new DriveTime. This command will drive your robot for a desired speed and time.
+
+        :param speed: The speed 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.speed = speed
+        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.getFPGATimestamp()
+        self.drive.arcadeDrive(0, 0)
+
+    def execute(self) -> None:
+        """Called every time the scheduler runs while the command is scheduled."""
+        self.drive.arcadeDrive(self.speed, 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.getFPGATimestamp() - self.startTime >= self.duration

robotpyExamples/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, speed: float, degrees: float, drive: Drivetrain) -> None:
+        """Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in
+        degrees) and rotational speed.
+
+        :param speed:   The speed 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.speed = speed
+        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.speed)
+
+    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/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 speed and time.
+    """
+
+    def __init__(self, speed: float, time: float, drive: Drivetrain) -> None:
+        """Creates a new TurnTime.
+
+        :param speed: The speed 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.rotationalSpeed = speed
+        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.getFPGATimestamp()
+        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.rotationalSpeed)
+
+    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.getFPGATimestamp() - self.startTime >= self.duration

robotpyExamples/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 testInit(self) -> None:
+        # Cancels all running commands at the start of test mode
+        commands2.CommandScheduler.getInstance().cancelAll()

robotpyExamples/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/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/SelectCommand/constants.py

@@ -0,0 +1,10 @@
+#
+# 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 OIConstants:
+    # Example: the port of the driver's controller
+    kDriverControllerPort = 0

robotpyExamples/SelectCommand/robot.py

@@ -0,0 +1,67 @@
+#!/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 commands2
+import typing
+
+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."""
+        pass
+
+    def disabledPeriodic(self) -> None:
+        """This function is called periodically when disabled"""
+        pass
+
+    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"""
+        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:
+            self.autonomousCommand.cancel()
+
+    def teleopPeriodic(self) -> None:
+        """This function is called periodically during operator control"""
+        pass
+
+    def testInit(self) -> None:
+        # Cancels all running commands at the start of test mode
+        commands2.CommandScheduler.getInstance().cancelAll()

robotpyExamples/SelectCommand/robotcontainer.py

@@ -0,0 +1,67 @@
+#
+# 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
+import commands2
+
+
+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.
+    """
+
+    # The enum used as keys for selecting the command to run.
+    class CommandSelector(enum.Enum):
+        ONE = enum.auto()
+        TWO = enum.auto()
+        THREE = enum.auto()
+
+    # An example selector method for the selectcommand.
+    def select(self) -> CommandSelector:
+        """Returns the selector that will select which command to run.
+        Can base this choice on logical conditions evaluated at runtime.
+        """
+        return self.CommandSelector.ONE
+
+    def __init__(self) -> None:
+        # An example selectcommand. Will select from the three commands based on the value returned
+        # by the selector method at runtime. Note that selectcommand takes a generic type, so the
+        # selector does not have to be an enum; it could be any desired type (string, integer,
+        # boolean, double...)
+        self.example_select_command = commands2.SelectCommand(
+            # Maps selector values to commands
+            {
+                self.CommandSelector.ONE: commands2.PrintCommand(
+                    "Command one was selected!"
+                ),
+                self.CommandSelector.TWO: commands2.PrintCommand(
+                    "Command two was selected!"
+                ),
+                self.CommandSelector.THREE: commands2.PrintCommand(
+                    "Command three was selected!"
+                ),
+            },
+            self.select,
+        )
+
+        # Configure the button bindings
+        self.configureButtonBindings()
+
+    def configureButtonBindings(self) -> None:
+        """Use this method to define your button->command mappings. Buttons can be created by
+        instantiating a {GenericHID} or one of its subclasses
+        ({edu.wpi.first.wpilibj.Joystick} or {XboxController}), and then calling passing it to a
+        {edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+        """
+
+    def getAutonomousCommand(self) -> commands2.Command:
+        """Use this to pass the autonomous command to the main {Robot} class.
+
+        :returns: the command to run in autonomous
+        """
+        return self.example_select_command

robotpyExamples/SimpleDifferentialDriveSimulation/drivetrain.py

@@ -0,0 +1,158 @@
+#
+# 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.
+    kMaxSpeed = 3.0
+    # 1/2 rotation per second.
+    kMaxAngularSpeed = 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.kFlat)
+
+        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 setSpeeds(self, speeds: wpimath.DifferentialDriveWheelSpeeds) -> None:
+        """Sets speeds to the drivetrain motors."""
+        leftFeedforward = self.feedforward.calculate(speeds.left)
+        rightFeedforward = self.feedforward.calculate(speeds.right)
+        leftOutput = self.leftPIDController.calculate(
+            self.leftEncoder.getRate(), speeds.left
+        )
+        rightOutput = self.rightPIDController.calculate(
+            self.rightEncoder.getRate(), speeds.right
+        )
+
+        self.leftLeader.setVoltage(leftOutput + leftFeedforward)
+        self.rightLeader.setVoltage(rightOutput + rightFeedforward)
+
+    def drive(self, xSpeed: float, rot: float) -> None:
+        """Controls the robot using arcade drive.
+
+        :param xSpeed: the speed for the x axis
+        :param rot: the rotation
+        """
+        self.setSpeeds(
+            self.kinematics.toWheelSpeeds(wpimath.ChassisSpeeds(xSpeed, 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.get() * wpilib.RobotController.getInputVoltage(),
+            self.rightLeader.get() * 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/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.speedLimiter = 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)
+        speeds = self.feedback.calculate(self.drive.getPose(), reference)
+        self.drive.drive(speeds.vx, speeds.omega)
+
+    def teleopPeriodic(self) -> None:
+        # Get the x speed. We are inverting this because Xbox controllers return
+        # negative values when we push forward.
+        xSpeed = (
+            -self.speedLimiter.calculate(self.controller.getLeftY())
+            * Drivetrain.kMaxSpeed
+        )
+
+        # 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.kMaxAngularSpeed
+        )
+        self.drive.drive(xSpeed, rot)
+
+    def simulationPeriodic(self) -> None:
+        self.drive.simulationPeriodic()