SCRIPT Move java files

wpilibjExamples/src/main/java/org/wpilib/commands/command2/ReplaceMeCommand.java

@@ -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.
+
+package edu.wpi.first.wpilibj.commands.command2;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class ReplaceMeCommand extends Command {
+  /** Creates a new ReplaceMeCommand. */
+  public ReplaceMeCommand() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/commands/commands.json

@@ -0,0 +1,82 @@
+[
+  {
+    "name": "Empty Class",
+    "description": "Create an empty class",
+    "tags": [
+      "class"
+    ],
+    "foldername": "emptyclass",
+    "replacename": "ReplaceMeEmptyClass",
+    "commandversion": 0
+  },
+  {
+    "name": "Command",
+    "description": "A command.",
+    "tags": [
+      "command"
+    ],
+    "foldername": "command2",
+    "replacename": "ReplaceMeCommand",
+    "commandversion": 2
+  },
+  {
+    "name": "InstantCommand",
+    "description": "A command that finishes instantly.",
+    "tags": [
+      "instantcommand"
+    ],
+    "foldername": "instantcommand",
+    "replacename": "ReplaceMeInstantCommand",
+    "commandversion": 2
+  },
+  {
+    "name": "ParallelCommandGroup",
+    "description": "A command group that runs commands in parallel, ending when all commands have finished.",
+    "tags": [
+      "parallelcommandgroup"
+    ],
+    "foldername": "parallelcommandgroup",
+    "replacename": "ReplaceMeParallelCommandGroup",
+    "commandversion": 2
+  },
+  {
+    "name": "ParallelDeadlineGroup",
+    "description": "A command group that runs commands in parallel, ending when a specific command has finished.",
+    "tags": [
+      "paralleldeadlinegroup"
+    ],
+    "foldername": "paralleldeadlinegroup",
+    "replacename": "ReplaceMeParallelDeadlineGroup",
+    "commandversion": 2
+  },
+  {
+    "name": "ParallelRaceGroup",
+    "description": "A command that runs commands in parallel, ending as soon as any command has finished.",
+    "tags": [
+      "parallelracegroup"
+    ],
+    "foldername": "parallelracegroup",
+    "replacename": "ReplaceMeParallelRaceGroup",
+    "commandversion": 2
+  },
+  {
+    "name": "SequentialCommandGroup",
+    "description": "A command group that runs commands in sequence.",
+    "tags": [
+      "sequentialcommandgroup"
+    ],
+    "foldername": "sequentialcommandgroup",
+    "replacename": "ReplaceMeSequentialCommandGroup",
+    "commandversion": 2
+  },
+  {
+    "name": "Subsystem",
+    "description": "A robot subsystem.",
+    "tags": [
+      "subsystem"
+    ],
+    "foldername": "subsystem2",
+    "replacename": "ReplaceMeSubsystem",
+    "commandversion": 2
+  }
+]

wpilibjExamples/src/main/java/org/wpilib/commands/emptyclass/ReplaceMeEmptyClass.java

@@ -0,0 +1,8 @@
+// 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.
+
+package edu.wpi.first.wpilibj.commands.emptyclass;
+
+/** Add your docs here. */
+public class ReplaceMeEmptyClass {}

wpilibjExamples/src/main/java/org/wpilib/commands/instantcommand/ReplaceMeInstantCommand.java

@@ -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.
+
+package edu.wpi.first.wpilibj.commands.instantcommand;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class ReplaceMeInstantCommand extends InstantCommand {
+  public ReplaceMeInstantCommand() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+}

wpilibjExamples/src/main/java/org/wpilib/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.java

@@ -0,0 +1,19 @@
+// 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.
+
+package edu.wpi.first.wpilibj.commands.parallelcommandgroup;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class ReplaceMeParallelCommandGroup extends ParallelCommandGroup {
+  /** Creates a new ReplaceMeParallelCommandGroup. */
+  public ReplaceMeParallelCommandGroup() {
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    addCommands();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.java

@@ -0,0 +1,21 @@
+// 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.
+
+package edu.wpi.first.wpilibj.commands.paralleldeadlinegroup;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class ReplaceMeParallelDeadlineGroup extends ParallelDeadlineGroup {
+  /** Creates a new ReplaceMeParallelDeadlineGroup. */
+  public ReplaceMeParallelDeadlineGroup() {
+    // Add the deadline command in the super() call. Add other commands using
+    // addCommands().
+    super(new InstantCommand());
+    // addCommands(new FooCommand(), new BarCommand());
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/commands/parallelracegroup/ReplaceMeParallelRaceGroup.java

@@ -0,0 +1,19 @@
+// 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.
+
+package edu.wpi.first.wpilibj.commands.parallelracegroup;
+
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class ReplaceMeParallelRaceGroup extends ParallelRaceGroup {
+  /** Creates a new ReplaceMeParallelRaceGroup. */
+  public ReplaceMeParallelRaceGroup() {
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    addCommands();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.java

@@ -0,0 +1,19 @@
+// 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.
+
+package edu.wpi.first.wpilibj.commands.sequentialcommandgroup;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class ReplaceMeSequentialCommandGroup extends SequentialCommandGroup {
+  /** Creates a new ReplaceMeSequentialCommandGroup. */
+  public ReplaceMeSequentialCommandGroup() {
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    addCommands();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/commands/subsystem2/ReplaceMeSubsystem.java

@@ -0,0 +1,17 @@
+// 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.
+
+package edu.wpi.first.wpilibj.commands.subsystem2;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class ReplaceMeSubsystem extends SubsystemBase {
+  /** Creates a new ReplaceMeSubsystem. */
+  public ReplaceMeSubsystem() {}
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/addressableled/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.addressableled;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/addressableled/Robot.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.addressableled;
+
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.LEDPattern;
+import edu.wpi.first.wpilibj.TimedRobot;
+
+public class Robot extends TimedRobot {
+  private final AddressableLED m_led;
+  private final AddressableLEDBuffer m_ledBuffer;
+
+  // Create an LED pattern that will display a rainbow across
+  // all hues at maximum saturation and half brightness
+  private final LEDPattern m_rainbow = LEDPattern.rainbow(255, 128);
+
+  // Our LED strip has a density of 120 LEDs per meter
+  private static final Distance kLedSpacing = Meters.of(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.
+  private final LEDPattern m_scrollingRainbow =
+      m_rainbow.scrollAtAbsoluteSpeed(MetersPerSecond.of(1), kLedSpacing);
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    // SmartIO port 1
+    m_led = new AddressableLED(1);
+
+    // Reuse buffer
+    // Default to a length of 60
+    m_ledBuffer = new AddressableLEDBuffer(60);
+    m_led.setLength(m_ledBuffer.getLength());
+
+    // Set the data
+    m_led.setData(m_ledBuffer);
+  }
+
+  @Override
+  public void robotPeriodic() {
+    // Update the buffer with the rainbow animation
+    m_scrollingRainbow.applyTo(m_ledBuffer);
+    // Set the LEDs
+    m_led.setData(m_ledBuffer);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/apriltagsvision/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.apriltagsvision;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/apriltagsvision/Robot.java

@@ -0,0 +1,149 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.apriltagsvision;
+
+import edu.wpi.first.apriltag.AprilTagDetection;
+import edu.wpi.first.apriltag.AprilTagDetector;
+import edu.wpi.first.apriltag.AprilTagPoseEstimator;
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.cscore.CvSink;
+import edu.wpi.first.cscore.CvSource;
+import edu.wpi.first.cscore.UsbCamera;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.networktables.IntegerArrayPublisher;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.TimedRobot;
+import java.util.ArrayList;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+
+/**
+ * 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.
+ *
+ * <p>Be aware that the performance on this is much worse than a coprocessor solution!
+ */
+public class Robot extends TimedRobot {
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    var visionThread = new Thread(this::apriltagVisionThreadProc);
+    visionThread.setDaemon(true);
+    visionThread.start();
+  }
+
+  void apriltagVisionThreadProc() {
+    var detector = new 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)
+    var poseEstConfig =
+        new AprilTagPoseEstimator.Config(
+            0.1651, 699.3778103158814, 677.7161226393544, 345.6059345433618, 207.12741326228522);
+    var estimator = new AprilTagPoseEstimator(poseEstConfig);
+
+    // Get the UsbCamera from CameraServer
+    UsbCamera camera = CameraServer.startAutomaticCapture();
+    // Set the resolution
+    camera.setResolution(640, 480);
+
+    // Get a CvSink. This will capture Mats from the camera
+    CvSink cvSink = CameraServer.getVideo();
+    // Setup a CvSource. This will send images back to the Dashboard
+    CvSource outputStream = CameraServer.putVideo("Detected", 640, 480);
+
+    // Mats are very memory expensive. Lets reuse these.
+    var mat = new Mat();
+    var grayMat = new Mat();
+
+    // Instantiate once
+    ArrayList<Long> tags = new ArrayList<>();
+    var outlineColor = new Scalar(0, 255, 0);
+    var crossColor = new Scalar(0, 0, 255);
+
+    // We'll output to NT
+    NetworkTable tagsTable = NetworkTableInstance.getDefault().getTable("apriltags");
+    IntegerArrayPublisher pubTags = tagsTable.getIntegerArrayTopic("tags").publish();
+
+    // This cannot be 'true'. The program will never exit if it is. This
+    // lets the robot stop this thread when restarting robot code or
+    // deploying.
+    while (!Thread.interrupted()) {
+      // 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;
+      }
+
+      Imgproc.cvtColor(mat, grayMat, Imgproc.COLOR_RGB2GRAY);
+
+      AprilTagDetection[] detections = detector.detect(grayMat);
+
+      // have not seen any tags yet
+      tags.clear();
+
+      for (AprilTagDetection detection : detections) {
+        // remember we saw this tag
+        tags.add((long) detection.getId());
+
+        // draw lines around the tag
+        for (var i = 0; i <= 3; i++) {
+          var j = (i + 1) % 4;
+          var pt1 = new Point(detection.getCornerX(i), detection.getCornerY(i));
+          var pt2 = new Point(detection.getCornerX(j), detection.getCornerY(j));
+          Imgproc.line(mat, pt1, pt2, outlineColor, 2);
+        }
+
+        // mark the center of the tag
+        var cx = detection.getCenterX();
+        var cy = detection.getCenterY();
+        var ll = 10;
+        Imgproc.line(mat, new Point(cx - ll, cy), new Point(cx + ll, cy), crossColor, 2);
+        Imgproc.line(mat, new Point(cx, cy - ll), new Point(cx, cy + ll), crossColor, 2);
+
+        // identify the tag
+        Imgproc.putText(
+            mat,
+            Integer.toString(detection.getId()),
+            new Point(cx + ll, cy),
+            Imgproc.FONT_HERSHEY_SIMPLEX,
+            1,
+            crossColor,
+            3);
+
+        // determine pose
+        Transform3d pose = estimator.estimate(detection);
+
+        // put pose into dashboard
+        Rotation3d rot = pose.getRotation();
+        tagsTable
+            .getEntry("pose_" + detection.getId())
+            .setDoubleArray(
+                new double[] {
+                  pose.getX(), pose.getY(), pose.getZ(), rot.getX(), rot.getY(), rot.getZ()
+                });
+      }
+
+      // put list of tags onto dashboard
+      pubTags.set(tags.stream().mapToLong(Long::longValue).toArray());
+
+      // Give the output stream a new image to display
+      outputStream.putFrame(mat);
+    }
+
+    pubTags.close();
+    detector.close();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/arcadedrive/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.arcadedrive;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/arcadedrive/Robot.java

@@ -0,0 +1,42 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.arcadedrive;
+
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+
+/**
+ * This is a demo program showing the use of the DifferentialDrive class. Runs the motors with
+ * arcade steering.
+ */
+public class Robot extends TimedRobot {
+  private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
+  private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
+  private final DifferentialDrive m_robotDrive =
+      new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
+  private final Joystick m_stick = new Joystick(0);
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    SendableRegistry.addChild(m_robotDrive, m_leftMotor);
+    SendableRegistry.addChild(m_robotDrive, m_rightMotor);
+
+    // 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.
+    m_rightMotor.setInverted(true);
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    // Drive with arcade drive.
+    // That means that the Y axis drives forward
+    // and backward, and the X turns left and right.
+    m_robotDrive.arcadeDrive(-m_stick.getY(), -m_stick.getX());
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/arcadedrivexboxcontroller/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.arcadedrivexboxcontroller;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/arcadedrivexboxcontroller/Robot.java

@@ -0,0 +1,42 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.arcadedrivexboxcontroller;
+
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+
+/**
+ * This is a demo program showing the use of the DifferentialDrive class. Runs the motors with split
+ * arcade steering and an Xbox controller.
+ */
+public class Robot extends TimedRobot {
+  private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
+  private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
+  private final DifferentialDrive m_robotDrive =
+      new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
+  private final XboxController m_driverController = new XboxController(0);
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    SendableRegistry.addChild(m_robotDrive, m_leftMotor);
+    SendableRegistry.addChild(m_robotDrive, m_rightMotor);
+
+    // 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.
+    m_rightMotor.setInverted(true);
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    // Drive with split arcade drive.
+    // 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.
+    m_robotDrive.arcadeDrive(-m_driverController.getLeftY(), -m_driverController.getRightX());
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/armsimulation/Constants.java

@@ -0,0 +1,31 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.armsimulation;
+
+import edu.wpi.first.math.util.Units;
+
+public class Constants {
+  public static final int kMotorPort = 0;
+  public static final int kEncoderAChannel = 0;
+  public static final int kEncoderBChannel = 1;
+  public static final int kJoystickPort = 0;
+
+  public static final String kArmPositionKey = "ArmPosition";
+  public static final String kArmPKey = "ArmP";
+
+  // The P gain for the PID controller that drives this arm.
+  public static final double kDefaultArmKp = 50.0;
+  public static final double kDefaultArmSetpointDegrees = 75.0;
+
+  // distance per pulse = (angle per revolution) / (pulses per revolution)
+  //  = (2 * PI rads) / (4096 pulses)
+  public static final double kArmEncoderDistPerPulse = 2.0 * Math.PI / 4096;
+
+  public static final double kArmReduction = 200;
+  public static final double kArmMass = 8.0; // Kilograms
+  public static final double kArmLength = Units.inchesToMeters(30);
+  public static final double kMinAngleRads = Units.degreesToRadians(-75);
+  public static final double kMaxAngleRads = Units.degreesToRadians(255);
+}

wpilibjExamples/src/main/java/org/wpilib/examples/armsimulation/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.armsimulation;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/armsimulation/Robot.java

@@ -0,0 +1,50 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.armsimulation;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.examples.armsimulation.subsystems.Arm;
+
+/** This is a sample program to demonstrate the use of arm simulation with existing code. */
+public class Robot extends TimedRobot {
+  private final Arm m_arm = new Arm();
+  private final Joystick m_joystick = new Joystick(Constants.kJoystickPort);
+
+  public Robot() {}
+
+  @Override
+  public void simulationPeriodic() {
+    m_arm.simulationPeriodic();
+  }
+
+  @Override
+  public void teleopInit() {
+    m_arm.loadPreferences();
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    if (m_joystick.getTrigger()) {
+      // Here, we run PID control like normal.
+      m_arm.reachSetpoint();
+    } else {
+      // Otherwise, we disable the motor.
+      m_arm.stop();
+    }
+  }
+
+  @Override
+  public void close() {
+    m_arm.close();
+    super.close();
+  }
+
+  @Override
+  public void disabledInit() {
+    // This just makes sure that our simulation code knows that the motor's off.
+    m_arm.stop();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/armsimulation/subsystems/Arm.java

@@ -0,0 +1,135 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.armsimulation.subsystems;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Preferences;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.examples.armsimulation.Constants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+public class Arm implements AutoCloseable {
+  // The P gain for the PID controller that drives this arm.
+  private double m_armKp = Constants.kDefaultArmKp;
+  private double m_armSetpointDegrees = Constants.kDefaultArmSetpointDegrees;
+
+  // The arm gearbox represents a gearbox containing two Vex 775pro motors.
+  private final DCMotor m_armGearbox = DCMotor.getVex775Pro(2);
+
+  // Standard classes for controlling our arm
+  private final PIDController m_controller = new PIDController(m_armKp, 0, 0);
+  private final Encoder m_encoder =
+      new Encoder(Constants.kEncoderAChannel, Constants.kEncoderBChannel);
+  private final PWMSparkMax m_motor = new 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).
+  private final SingleJointedArmSim m_armSim =
+      new SingleJointedArmSim(
+          m_armGearbox,
+          Constants.kArmReduction,
+          SingleJointedArmSim.estimateMOI(Constants.kArmLength, Constants.kArmMass),
+          Constants.kArmLength,
+          Constants.kMinAngleRads,
+          Constants.kMaxAngleRads,
+          true,
+          0,
+          Constants.kArmEncoderDistPerPulse,
+          0.0 // Add noise with a std-dev of 1 tick
+          );
+  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
+
+  // Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm.
+  private final Mechanism2d m_mech2d = new Mechanism2d(60, 60);
+  private final MechanismRoot2d m_armPivot = m_mech2d.getRoot("ArmPivot", 30, 30);
+  private final MechanismLigament2d m_armTower =
+      m_armPivot.append(new MechanismLigament2d("ArmTower", 30, -90));
+  private final MechanismLigament2d m_arm =
+      m_armPivot.append(
+          new MechanismLigament2d(
+              "Arm",
+              30,
+              Units.radiansToDegrees(m_armSim.getAngle()),
+              6,
+              new Color8Bit(Color.kYellow)));
+
+  /** Subsystem constructor. */
+  public Arm() {
+    m_encoder.setDistancePerPulse(Constants.kArmEncoderDistPerPulse);
+
+    // Put Mechanism 2d to SmartDashboard
+    SmartDashboard.putData("Arm Sim", m_mech2d);
+    m_armTower.setColor(new Color8Bit(Color.kBlue));
+
+    // Set the Arm position setpoint and P constant to Preferences if the keys don't already exist
+    Preferences.initDouble(Constants.kArmPositionKey, m_armSetpointDegrees);
+    Preferences.initDouble(Constants.kArmPKey, m_armKp);
+  }
+
+  /** Update the simulation model. */
+  public void simulationPeriodic() {
+    // In this method, we update our simulation of what our arm is doing
+    // First, we set our "inputs" (voltages)
+    m_armSim.setInput(m_motor.get() * RobotController.getBatteryVoltage());
+
+    // Next, we update it. The standard loop time is 20ms.
+    m_armSim.update(0.020);
+
+    // Finally, we set our simulated encoder's readings and simulated battery voltage
+    m_encoderSim.setDistance(m_armSim.getAngle());
+    // SimBattery estimates loaded battery voltages
+    RoboRioSim.setVInVoltage(
+        BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDraw()));
+
+    // Update the Mechanism Arm angle based on the simulated arm angle
+    m_arm.setAngle(Units.radiansToDegrees(m_armSim.getAngle()));
+  }
+
+  /** Load setpoint and kP from preferences. */
+  public void loadPreferences() {
+    // Read Preferences for Arm setpoint and kP on entering Teleop
+    m_armSetpointDegrees = Preferences.getDouble(Constants.kArmPositionKey, m_armSetpointDegrees);
+    if (m_armKp != Preferences.getDouble(Constants.kArmPKey, m_armKp)) {
+      m_armKp = Preferences.getDouble(Constants.kArmPKey, m_armKp);
+      m_controller.setP(m_armKp);
+    }
+  }
+
+  /** Run the control loop to reach and maintain the setpoint from the preferences. */
+  public void reachSetpoint() {
+    var pidOutput =
+        m_controller.calculate(
+            m_encoder.getDistance(), Units.degreesToRadians(m_armSetpointDegrees));
+    m_motor.setVoltage(pidOutput);
+  }
+
+  public void stop() {
+    m_motor.set(0.0);
+  }
+
+  @Override
+  public void close() {
+    m_motor.close();
+    m_encoder.close();
+    m_mech2d.close();
+    m_armPivot.close();
+    m_controller.close();
+    m_arm.close();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/canpdp/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.canpdp;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/canpdp/Robot.java

@@ -0,0 +1,54 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.canpdp;
+
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * 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.
+ */
+public class Robot extends TimedRobot {
+  private final PowerDistribution m_pdp = new PowerDistribution(0);
+
+  public Robot() {
+    // Put the PDP itself to the dashboard
+    SmartDashboard.putData("PDP", m_pdp);
+  }
+
+  @Override
+  public void robotPeriodic() {
+    // 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.
+    double current7 = m_pdp.getCurrent(7);
+    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.
+    double voltage = m_pdp.getVoltage();
+    SmartDashboard.putNumber("Voltage", voltage);
+
+    // Retrieves the temperature of the PDP, in degrees Celsius.
+    double temperatureCelsius = m_pdp.getTemperature();
+    SmartDashboard.putNumber("Temperature", temperatureCelsius);
+
+    // Get the total current of all channels.
+    double totalCurrent = m_pdp.getTotalCurrent();
+    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.
+    double totalPower = m_pdp.getTotalPower();
+    SmartDashboard.putNumber("Total Power", totalPower);
+
+    // Get the total energy of all channels.
+    // Energy is the power summed over time with units Joules.
+    double totalEnergy = m_pdp.getTotalEnergy();
+    SmartDashboard.putNumber("Total Energy", totalEnergy);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/differentialdrivebot/Drivetrain.java

@@ -0,0 +1,108 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.differentialdrivebot;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.OnboardIMU;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+
+/** Represents a differential drive style drivetrain. */
+public class Drivetrain {
+  public static final double kMaxSpeed = 3.0; // meters per second
+  public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
+
+  private static final double kTrackwidth = 0.381 * 2; // meters
+  private static final double kWheelRadius = 0.0508; // meters
+  private static final int kEncoderResolution = 4096;
+
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(1);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(2);
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(3);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(4);
+
+  private final Encoder m_leftEncoder = new Encoder(0, 1);
+  private final Encoder m_rightEncoder = new Encoder(2, 3);
+
+  private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.kFlat);
+
+  private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
+
+  private final DifferentialDriveKinematics m_kinematics =
+      new DifferentialDriveKinematics(kTrackwidth);
+
+  private final DifferentialDriveOdometry m_odometry;
+
+  // Gains are for example purposes only - must be determined for your own robot!
+  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
+
+  /**
+   * Constructs a differential drive object. Sets the encoder distance per pulse and resets the
+   * gyro.
+   */
+  public Drivetrain() {
+    m_imu.resetYaw();
+
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_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.
+    m_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.
+    m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+    m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+
+    m_odometry =
+        new DifferentialDriveOdometry(
+            m_imu.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
+  }
+
+  /**
+   * Sets the desired wheel speeds.
+   *
+   * @param speeds The desired wheel speeds.
+   */
+  public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
+    final double leftFeedforward = m_feedforward.calculate(speeds.left);
+    final double rightFeedforward = m_feedforward.calculate(speeds.right);
+
+    final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.left);
+    final double rightOutput =
+        m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.right);
+    m_leftLeader.setVoltage(leftOutput + leftFeedforward);
+    m_rightLeader.setVoltage(rightOutput + rightFeedforward);
+  }
+
+  /**
+   * 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.
+   */
+  public void drive(double xSpeed, double rot) {
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot));
+    setSpeeds(wheelSpeeds);
+  }
+
+  /** Updates the field-relative position. */
+  public void updateOdometry() {
+    m_odometry.update(
+        m_imu.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/differentialdrivebot/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.differentialdrivebot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/differentialdrivebot/Robot.java

@@ -0,0 +1,39 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.differentialdrivebot;
+
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class Robot extends TimedRobot {
+  private final XboxController m_controller = new XboxController(0);
+  private final Drivetrain m_drive = new Drivetrain();
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+  private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
+  @Override
+  public void autonomousPeriodic() {
+    teleopPeriodic();
+    m_drive.updateOdometry();
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    final var xSpeed = -m_speedLimiter.calculate(m_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.
+    final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
+
+    m_drive.drive(xSpeed, rot);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/Drivetrain.java

@@ -0,0 +1,267 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.ComputerVisionUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.DoubleArrayEntry;
+import edu.wpi.first.networktables.DoubleArrayTopic;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.OnboardIMU;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/** Represents a differential drive style drivetrain. */
+public class Drivetrain {
+  public static final double kMaxSpeed = 3.0; // meters per second
+  public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
+
+  private static final double kTrackwidth = 0.381 * 2; // meters
+  private static final double kWheelRadius = 0.0508; // meters
+  private static final int kEncoderResolution = 4096;
+
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(1);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(2);
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(3);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(4);
+
+  private final Encoder m_leftEncoder = new Encoder(0, 1);
+  private final Encoder m_rightEncoder = new Encoder(2, 3);
+
+  private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.kFlat);
+
+  private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
+
+  private final DifferentialDriveKinematics m_kinematics =
+      new DifferentialDriveKinematics(kTrackwidth);
+
+  private final Pose3d m_objectInField;
+
+  private final Transform3d m_robotToCamera =
+      new Transform3d(new Translation3d(1, 1, 1), new Rotation3d(0, 0, Math.PI / 2));
+
+  private final DoubleArrayEntry m_cameraToObjectEntry;
+
+  private final double[] m_defaultVal = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+
+  private final Field2d m_fieldSim = new Field2d();
+  private final Field2d m_fieldApproximation = new Field2d();
+
+  /* Here we use DifferentialDrivePoseEstimator so that we can fuse odometry readings. The
+  numbers used  below are robot specific, and should be tuned. */
+  private final DifferentialDrivePoseEstimator m_poseEstimator =
+      new DifferentialDrivePoseEstimator(
+          m_kinematics,
+          m_imu.getRotation2d(),
+          m_leftEncoder.getDistance(),
+          m_rightEncoder.getDistance(),
+          Pose2d.kZero,
+          VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
+          VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+
+  // Gains are for example purposes only - must be determined for your own robot!
+  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
+
+  // Simulation classes
+  private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder);
+  private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder);
+  private final LinearSystem<N2, N2, N2> m_drivetrainSystem =
+      LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
+  private final DifferentialDrivetrainSim m_drivetrainSimulator =
+      new DifferentialDrivetrainSim(
+          m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackwidth, kWheelRadius, null);
+
+  /**
+   * Constructs a differential drive object. Sets the encoder distance per pulse and resets the
+   * gyro.
+   */
+  public Drivetrain(DoubleArrayTopic cameraToObjectTopic) {
+    m_imu.resetYaw();
+
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_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.
+    m_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.
+    m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+    m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+
+    m_cameraToObjectEntry = cameraToObjectTopic.getEntry(m_defaultVal);
+
+    m_objectInField =
+        AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo).getTagPose(0).get();
+
+    SmartDashboard.putData("Field", m_fieldSim);
+    SmartDashboard.putData("FieldEstimation", m_fieldApproximation);
+  }
+
+  /**
+   * Sets the desired wheel speeds.
+   *
+   * @param speeds The desired wheel speeds.
+   */
+  public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
+    final double leftFeedforward = m_feedforward.calculate(speeds.left);
+    final double rightFeedforward = m_feedforward.calculate(speeds.right);
+
+    final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.left);
+    final double rightOutput =
+        m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.right);
+    m_leftLeader.setVoltage(leftOutput + leftFeedforward);
+    m_rightLeader.setVoltage(rightOutput + rightFeedforward);
+  }
+
+  /**
+   * 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.
+   */
+  public void drive(double xSpeed, double rot) {
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot));
+    setSpeeds(wheelSpeeds);
+  }
+
+  /**
+   * 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.
+   *
+   * <p>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.
+   */
+  public void publishCameraToObject(
+      Pose3d objectInField,
+      Transform3d robotToCamera,
+      DoubleArrayEntry cameraToObjectEntry,
+      DifferentialDrivetrainSim drivetrainSimulator) {
+    Pose3d robotInField = new Pose3d(drivetrainSimulator.getPose());
+    Pose3d cameraInField = robotInField.plus(robotToCamera);
+    Transform3d cameraToObject = new Transform3d(cameraInField, objectInField);
+
+    // Publishes double array with Translation3D elements {x, y, z} and Rotation3D elements {w, x,
+    // y, z} which describe
+    // the cameraToObject transformation.
+    double[] val = {
+      cameraToObject.getX(),
+      cameraToObject.getY(),
+      cameraToObject.getZ(),
+      cameraToObject.getRotation().getQuaternion().getW(),
+      cameraToObject.getRotation().getQuaternion().getX(),
+      cameraToObject.getRotation().getQuaternion().getY(),
+      cameraToObject.getRotation().getQuaternion().getZ()
+    };
+    cameraToObjectEntry.set(val);
+  }
+
+  /**
+   * Queries the camera-to-object transformation from networktables to compute the robot's
+   * field-relative pose from vision measurements.
+   *
+   * <p>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.
+   */
+  public Pose3d objectToRobotPose(
+      Pose3d objectInField, Transform3d robotToCamera, DoubleArrayEntry cameraToObjectEntry) {
+    double[] val = cameraToObjectEntry.get();
+
+    // Reconstruct cameraToObject Transform3D from networktables.
+    Translation3d translation = new Translation3d(val[0], val[1], val[2]);
+    Rotation3d rotation = new Rotation3d(new Quaternion(val[3], val[4], val[5], val[6]));
+    Transform3d cameraToObject = new Transform3d(translation, rotation);
+
+    return ComputerVisionUtil.objectToRobotPose(objectInField, cameraToObject, robotToCamera);
+  }
+
+  /** Updates the field-relative position. */
+  public void updateOdometry() {
+    m_poseEstimator.update(
+        m_imu.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
+
+    // Publish cameraToObject transformation to networktables --this would normally be handled by
+    // the
+    // computer vision solution.
+    publishCameraToObject(
+        m_objectInField, m_robotToCamera, m_cameraToObjectEntry, m_drivetrainSimulator);
+
+    // Compute the robot's field-relative position exclusively from vision measurements.
+    Pose3d visionMeasurement3d =
+        objectToRobotPose(m_objectInField, m_robotToCamera, m_cameraToObjectEntry);
+
+    // Convert robot pose from Pose3d to Pose2d needed to apply vision measurements.
+    Pose2d 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.
+    m_poseEstimator.addVisionMeasurement(visionMeasurement2d, Timer.getTimestamp());
+  }
+
+  /** This function is called periodically during simulation. */
+  public void simulationPeriodic() {
+    // 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.
+    m_drivetrainSimulator.setInputs(
+        m_leftLeader.get() * RobotController.getInputVoltage(),
+        m_rightLeader.get() * RobotController.getInputVoltage());
+    m_drivetrainSimulator.update(0.02);
+
+    m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPosition());
+    m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocity());
+    m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPosition());
+    m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocity());
+    // m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees()); // TODO(Ryan): fixup
+    // when sim implemented
+  }
+
+  /** This function is called periodically, no matter the mode. */
+  public void periodic() {
+    updateOdometry();
+    m_fieldSim.setRobotPose(m_drivetrainSimulator.getPose());
+    m_fieldApproximation.setRobotPose(m_poseEstimator.getEstimatedPosition());
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/ExampleGlobalMeasurementSensor.java

@@ -0,0 +1,32 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
+
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+
+/** This dummy class represents a global measurement sensor, such as a computer vision solution. */
+public final class ExampleGlobalMeasurementSensor {
+  private ExampleGlobalMeasurementSensor() {
+    // Utility class
+  }
+
+  /**
+   * Get a "noisy" fake global pose reading.
+   *
+   * @param estimatedRobotPose The robot pose.
+   */
+  public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
+    var rand =
+        StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+    return new Pose2d(
+        estimatedRobotPose.getX() + rand.get(0, 0),
+        estimatedRobotPose.getY() + rand.get(1, 0),
+        estimatedRobotPose.getRotation().plus(new Rotation2d(rand.get(2, 0))));
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/Robot.java

@@ -0,0 +1,55 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
+
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.networktables.DoubleArrayTopic;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class Robot extends TimedRobot {
+  private final NetworkTableInstance m_inst = NetworkTableInstance.getDefault();
+  private final DoubleArrayTopic m_doubleArrayTopic =
+      m_inst.getDoubleArrayTopic("m_doubleArrayTopic");
+
+  private final XboxController m_controller = new XboxController(0);
+  private final Drivetrain m_drive = new Drivetrain(m_doubleArrayTopic);
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+  private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
+  @Override
+  public void autonomousPeriodic() {
+    teleopPeriodic();
+    m_drive.updateOdometry();
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    m_drive.simulationPeriodic();
+  }
+
+  @Override
+  public void robotPeriodic() {
+    m_drive.periodic();
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    final var xSpeed = -m_speedLimiter.calculate(m_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.
+    final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
+
+    m_drive.drive(xSpeed, rot);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/digitalcommunication/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.digitalcommunication;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/digitalcommunication/Robot.java

@@ -0,0 +1,59 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.digitalcommunication;
+
+import edu.wpi.first.wpilibj.DigitalOutput;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.TimedRobot;
+import java.util.Optional;
+
+/**
+ * This is a sample program demonstrating how to communicate to a light controller from the robot
+ * code using the roboRIO's DIO ports.
+ */
+public class Robot extends TimedRobot {
+  // define ports for digitalcommunication with light controller
+  static final int kAlliancePort = 0;
+  static final int kEnabledPort = 1;
+  static final int kAutonomousPort = 2;
+  static final int kAlertPort = 3;
+
+  private final DigitalOutput m_allianceOutput = new DigitalOutput(kAlliancePort);
+  private final DigitalOutput m_enabledOutput = new DigitalOutput(kEnabledPort);
+  private final DigitalOutput m_autonomousOutput = new DigitalOutput(kAutonomousPort);
+  private final DigitalOutput m_alertOutput = new DigitalOutput(kAlertPort);
+
+  @Override
+  public void robotPeriodic() {
+    boolean setAlliance = false;
+    Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
+    if (alliance.isPresent()) {
+      setAlliance = alliance.get() == DriverStation.Alliance.Red;
+    }
+
+    // pull alliance port high if on red alliance, pull low if on blue alliance
+    m_allianceOutput.set(setAlliance);
+
+    // pull enabled port high if enabled, low if disabled
+    m_enabledOutput.set(DriverStation.isEnabled());
+
+    // pull auto port high if in autonomous, low if in teleop (or disabled)
+    m_autonomousOutput.set(DriverStation.isAutonomous());
+
+    // pull alert port high if match time remaining is between 30 and 25 seconds
+    var matchTime = DriverStation.getMatchTime();
+    m_alertOutput.set(matchTime <= 30 && matchTime >= 25);
+  }
+
+  /** Close all resources. */
+  @Override
+  public void close() {
+    m_allianceOutput.close();
+    m_enabledOutput.close();
+    m_autonomousOutput.close();
+    m_alertOutput.close();
+    super.close();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/drivedistanceoffboard/Constants.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+  public static final class DriveConstants {
+    public static final double kDt = 0.02;
+    public static final int kLeftMotor1Port = 0;
+    public static final int kLeftMotor2Port = 1;
+    public static final int kRightMotor1Port = 2;
+    public static final int 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.
+    public static final double ks = 1; // V
+    public static final double kv = 0.8; // V/(m/s)
+    public static final double ka = 0.15; // V/(m/s²)
+
+    public static final double kp = 1;
+
+    public static final double kMaxSpeed = 3; // m/s
+    public static final double kMaxAcceleration = 3; // m/s²
+  }
+
+  public static final class OIConstants {
+    public static final int kDriverControllerPort = 0;
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/drivedistanceoffboard/ExampleSmartMotorController.java

@@ -0,0 +1,92 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor controller.
+ *
+ * <p>Has no actual functionality.
+ */
+public class ExampleSmartMotorController {
+  public enum PIDMode {
+    kPosition,
+    kVelocity,
+    kMovementWitchcraft
+  }
+
+  double m_value;
+
+  /**
+   * Creates a new ExampleSmartMotorController.
+   *
+   * @param port The port for the controller.
+   */
+  @SuppressWarnings("PMD.UnusedFormalParameter")
+  public ExampleSmartMotorController(int port) {}
+
+  /**
+   * 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.
+   */
+  public void setPID(double kp, double ki, double kd) {}
+
+  /**
+   * 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).
+   */
+  public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
+
+  /**
+   * Places this motor controller in follower mode.
+   *
+   * @param leader The leader to follow.
+   */
+  public void follow(ExampleSmartMotorController leader) {}
+
+  /**
+   * Returns the encoder distance.
+   *
+   * @return The current encoder distance.
+   */
+  public double getEncoderDistance() {
+    return 0;
+  }
+
+  /**
+   * Returns the encoder rate.
+   *
+   * @return The current encoder rate.
+   */
+  public double getEncoderRate() {
+    return 0;
+  }
+
+  /** Resets the encoder to zero distance. */
+  public void resetEncoder() {}
+
+  public void set(double speed) {
+    m_value = speed;
+  }
+
+  public double get() {
+    return m_value;
+  }
+
+  public void setInverted(boolean isInverted) {}
+
+  public boolean getInverted() {
+    return false;
+  }
+
+  public void disable() {}
+
+  public void stopMotor() {}
+}

wpilibjExamples/src/main/java/org/wpilib/examples/drivedistanceoffboard/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/drivedistanceoffboard/Robot.java

@@ -0,0 +1,100 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * 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.
+ */
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+
+  private final RobotContainer m_robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  public Robot() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+  }
+
+  /**
+   * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
+   * that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /** This function is called once each time the robot enters Disabled mode. */
+  @Override
+  public void disabledInit() {}
+
+  @Override
+  public void disabledPeriodic() {}
+
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    /*
+     * String autoSelected = SmartDashboard.getString("Auto Selector",
+     * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+     * = new MyAutoCommand(); break; case "Default Auto": default:
+     * autonomousCommand = new ExampleCommand(); break; }
+     */
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null) {
+      CommandScheduler.getInstance().schedule(m_autonomousCommand);
+    }
+  }
+
+  /** This function is called periodically during autonomous. */
+  @Override
+  public void autonomousPeriodic() {}
+
+  @Override
+  public void teleopInit() {
+    // 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 (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  /** This function is called periodically during operator control. */
+  @Override
+  public void teleopPeriodic() {}
+
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  /** This function is called periodically during test mode. */
+  @Override
+  public void testPeriodic() {}
+}

wpilibjExamples/src/main/java/org/wpilib/examples/drivedistanceoffboard/RobotContainer.java

@@ -0,0 +1,74 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.OIConstants;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+
+/**
+ * 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 {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+  // The robot's subsystems
+  private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+
+  // Retained command references
+  private final Command m_driveFullSpeed = Commands.runOnce(() -> m_robotDrive.setMaxOutput(1));
+  private final Command m_driveHalfSpeed = Commands.runOnce(() -> m_robotDrive.setMaxOutput(0.5));
+
+  // The driver's controller
+  CommandXboxController m_driverController =
+      new CommandXboxController(OIConstants.kDriverControllerPort);
+
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
+  public RobotContainer() {
+    // Configure the button bindings
+    configureButtonBindings();
+
+    // Configure default commands
+    // Set the default drive command to split-stick arcade drive
+    m_robotDrive.setDefaultCommand(
+        // A split-stick arcade command, with forward/backward controlled by the left
+        // hand, and turning controlled by the right.
+        Commands.run(
+            () ->
+                m_robotDrive.arcadeDrive(
+                    -m_driverController.getLeftY(), -m_driverController.getRightX()),
+            m_robotDrive));
+  }
+
+  /**
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   */
+  private void configureButtonBindings() {
+    // Drive at half speed when the bumper is held
+    m_driverController.rightBumper().onTrue(m_driveHalfSpeed).onFalse(m_driveFullSpeed);
+
+    // Drive forward by 3 meters when the 'A' button is pressed, with a timeout of 10 seconds
+    m_driverController.a().onTrue(m_robotDrive.profiledDriveDistance(3).withTimeout(10));
+
+    // Do the same thing as above when the 'B' button is pressed, but without resetting the encoders
+    m_driverController.b().onTrue(m_robotDrive.dynamicProfiledDriveDistance(3).withTimeout(10));
+  }
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    return Commands.none();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java

@@ -0,0 +1,212 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.ExampleSmartMotorController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class DriveSubsystem extends SubsystemBase {
+  // The motors on the left side of the drive.
+  private final ExampleSmartMotorController m_leftLeader =
+      new ExampleSmartMotorController(DriveConstants.kLeftMotor1Port);
+
+  private final ExampleSmartMotorController m_leftFollower =
+      new ExampleSmartMotorController(DriveConstants.kLeftMotor2Port);
+
+  // The motors on the right side of the drive.
+  private final ExampleSmartMotorController m_rightLeader =
+      new ExampleSmartMotorController(DriveConstants.kRightMotor1Port);
+
+  private final ExampleSmartMotorController m_rightFollower =
+      new ExampleSmartMotorController(DriveConstants.kRightMotor2Port);
+
+  // The feedforward controller.
+  private final SimpleMotorFeedforward m_feedforward =
+      new SimpleMotorFeedforward(DriveConstants.ks, DriveConstants.kv, DriveConstants.ka);
+
+  // The robot's drive
+  private final DifferentialDrive m_drive =
+      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
+
+  // The trapezoid profile
+  private final TrapezoidProfile m_profile =
+      new TrapezoidProfile(
+          new TrapezoidProfile.Constraints(
+              DriveConstants.kMaxSpeed, DriveConstants.kMaxAcceleration));
+
+  // The timer
+  private final Timer m_timer = new Timer();
+
+  /** Creates a new DriveSubsystem. */
+  public DriveSubsystem() {
+    SendableRegistry.addChild(m_drive, m_leftLeader);
+    SendableRegistry.addChild(m_drive, m_rightLeader);
+
+    // 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.
+    m_rightLeader.setInverted(true);
+
+    m_leftFollower.follow(m_leftLeader);
+    m_rightFollower.follow(m_rightLeader);
+
+    m_leftLeader.setPID(DriveConstants.kp, 0, 0);
+    m_rightLeader.setPID(DriveConstants.kp, 0, 0);
+  }
+
+  /**
+   * Drives the robot using arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  public void arcadeDrive(double fwd, double rot) {
+    m_drive.arcadeDrive(fwd, rot);
+  }
+
+  /**
+   * 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.
+   */
+  public void setDriveStates(
+      TrapezoidProfile.State currentLeft,
+      TrapezoidProfile.State currentRight,
+      TrapezoidProfile.State nextLeft,
+      TrapezoidProfile.State nextRight) {
+    // Feedforward is divided by battery voltage to normalize it to [-1, 1]
+    m_leftLeader.setSetpoint(
+        ExampleSmartMotorController.PIDMode.kPosition,
+        currentLeft.position,
+        m_feedforward.calculate(currentLeft.velocity, nextLeft.velocity)
+            / RobotController.getBatteryVoltage());
+    m_rightLeader.setSetpoint(
+        ExampleSmartMotorController.PIDMode.kPosition,
+        currentRight.position,
+        m_feedforward.calculate(currentLeft.velocity, nextLeft.velocity)
+            / RobotController.getBatteryVoltage());
+  }
+
+  /**
+   * Returns the left encoder distance.
+   *
+   * @return the left encoder distance
+   */
+  public double getLeftEncoderDistance() {
+    return m_leftLeader.getEncoderDistance();
+  }
+
+  /**
+   * Returns the right encoder distance.
+   *
+   * @return the right encoder distance
+   */
+  public double getRightEncoderDistance() {
+    return m_rightLeader.getEncoderDistance();
+  }
+
+  /** Resets the drive encoders. */
+  public void resetEncoders() {
+    m_leftLeader.resetEncoder();
+    m_rightLeader.resetEncoder();
+  }
+
+  /**
+   * 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
+   */
+  public void setMaxOutput(double maxOutput) {
+    m_drive.setMaxOutput(maxOutput);
+  }
+
+  /**
+   * Creates a command to drive forward a specified distance using a motion profile.
+   *
+   * @param distance The distance to drive forward.
+   * @return A command.
+   */
+  public Command profiledDriveDistance(double distance) {
+    return startRun(
+            () -> {
+              // Restart timer so profile setpoints start at the beginning
+              m_timer.restart();
+              resetEncoders();
+            },
+            () -> {
+              // Current state never changes, so we need to use a timer to get the setpoints we need
+              // to be at
+              var currentTime = m_timer.get();
+              var currentSetpoint =
+                  m_profile.calculate(currentTime, new State(), new State(distance, 0));
+              var nextSetpoint =
+                  m_profile.calculate(
+                      currentTime + DriveConstants.kDt, new State(), new State(distance, 0));
+              setDriveStates(currentSetpoint, currentSetpoint, nextSetpoint, nextSetpoint);
+            })
+        .until(() -> m_profile.isFinished(0));
+  }
+
+  private double m_initialLeftDistance;
+  private double m_initialRightDistance;
+
+  /**
+   * Creates a command to drive forward a specified distance using a motion profile without
+   * resetting the encoders.
+   *
+   * @param distance The distance to drive forward.
+   * @return A command.
+   */
+  public Command dynamicProfiledDriveDistance(double distance) {
+    return startRun(
+            () -> {
+              // Restart timer so profile setpoints start at the beginning
+              m_timer.restart();
+              // Store distance so we know the target distance for each encoder
+              m_initialLeftDistance = getLeftEncoderDistance();
+              m_initialRightDistance = getRightEncoderDistance();
+            },
+            () -> {
+              // Current state never changes for the duration of the command, so we need to use a
+              // timer to get the setpoints we need to be at
+              var currentTime = m_timer.get();
+              var currentLeftSetpoint =
+                  m_profile.calculate(
+                      currentTime,
+                      new State(m_initialLeftDistance, 0),
+                      new State(m_initialLeftDistance + distance, 0));
+              var currentRightSetpoint =
+                  m_profile.calculate(
+                      currentTime,
+                      new State(m_initialRightDistance, 0),
+                      new State(m_initialRightDistance + distance, 0));
+              var nextLeftSetpoint =
+                  m_profile.calculate(
+                      currentTime + DriveConstants.kDt,
+                      new State(m_initialLeftDistance, 0),
+                      new State(m_initialLeftDistance + distance, 0));
+              var nextRightSetpoint =
+                  m_profile.calculate(
+                      currentTime + DriveConstants.kDt,
+                      new State(m_initialRightDistance, 0),
+                      new State(m_initialRightDistance + distance, 0));
+              setDriveStates(
+                  currentLeftSetpoint, currentRightSetpoint, nextLeftSetpoint, nextRightSetpoint);
+            })
+        .until(() -> m_profile.isFinished(0));
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/dutycycleencoder/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.dutycycleencoder;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/dutycycleencoder/Robot.java

@@ -0,0 +1,72 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.dutycycleencoder;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/** This example shows how to use a duty cycle encoder for devices such as an arm or elevator. */
+public class Robot extends TimedRobot {
+  private final DutyCycleEncoder m_dutyCycleEncoder;
+  private static final double m_fullRange = 1.3;
+  private static final double m_expectedZero = 0;
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    // 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.
+    m_dutyCycleEncoder = new DutyCycleEncoder(0, m_fullRange, m_expectedZero);
+
+    // 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.
+    m_dutyCycleEncoder.setAssumedFrequency(967.8);
+  }
+
+  @Override
+  public void robotPeriodic() {
+    // Connected can be checked, and uses the frequency of the encoder
+    boolean connected = m_dutyCycleEncoder.isConnected();
+
+    // Duty Cycle Frequency in Hz
+    double frequency = m_dutyCycleEncoder.getFrequency();
+
+    // Output of encoder
+    double output = m_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.
+    double percentOfRange = m_fullRange * 0.1;
+    double shiftedOutput =
+        MathUtil.inputModulus(output, 0 - percentOfRange, m_fullRange - percentOfRange);
+
+    SmartDashboard.putBoolean("Connected", connected);
+    SmartDashboard.putNumber("Frequency", frequency);
+    SmartDashboard.putNumber("Output", output);
+    SmartDashboard.putNumber("ShiftedOutput", shiftedOutput);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/dutycycleinput/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.dutycycleinput;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/dutycycleinput/Robot.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.dutycycleinput;
+
+import edu.wpi.first.wpilibj.DutyCycle;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class Robot extends TimedRobot {
+  private final DutyCycle m_dutyCycle;
+
+  public Robot() {
+    m_dutyCycle = new DutyCycle(0);
+  }
+
+  @Override
+  public void robotPeriodic() {
+    // Duty Cycle Frequency in Hz
+    double frequency = m_dutyCycle.getFrequency();
+
+    // Output of duty cycle, ranging from 0 to 1
+    // 1 is fully on, 0 is fully off
+    double output = m_dutyCycle.getOutput();
+
+    SmartDashboard.putNumber("Frequency", frequency);
+    SmartDashboard.putNumber("Duty Cycle", output);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/elevatorexponentialprofile/ExampleSmartMotorController.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.elevatorexponentialprofile;
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor controller.
+ *
+ * <p>Has no actual functionality.
+ */
+public class ExampleSmartMotorController {
+  public enum PIDMode {
+    kPosition,
+    kVelocity,
+    kMovementWitchcraft
+  }
+
+  /**
+   * Creates a new ExampleSmartMotorController.
+   *
+   * @param port The port for the controller.
+   */
+  @SuppressWarnings("PMD.UnusedFormalParameter")
+  public ExampleSmartMotorController(int port) {}
+
+  /**
+   * 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.
+   */
+  public void setPID(double kp, double ki, double kd) {}
+
+  /**
+   * 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).
+   */
+  public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
+
+  /**
+   * Places this motor controller in follower mode.
+   *
+   * @param leader The leader to follow.
+   */
+  public void follow(ExampleSmartMotorController leader) {}
+
+  /**
+   * Returns the encoder distance.
+   *
+   * @return The current encoder distance.
+   */
+  public double getEncoderDistance() {
+    return 0;
+  }
+
+  /**
+   * Returns the encoder rate.
+   *
+   * @return The current encoder rate.
+   */
+  public double getEncoderRate() {
+    return 0;
+  }
+
+  /** Resets the encoder to zero distance. */
+  public void resetEncoder() {}
+
+  public void set(double speed) {}
+
+  public double get() {
+    return 0;
+  }
+
+  public void setInverted(boolean isInverted) {}
+
+  public boolean getInverted() {
+    return false;
+  }
+
+  public void disable() {}
+
+  public void stopMotor() {}
+}

wpilibjExamples/src/main/java/org/wpilib/examples/elevatorexponentialprofile/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorexponentialprofile;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/elevatorexponentialprofile/Robot.java

@@ -0,0 +1,52 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorexponentialprofile;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.trajectory.ExponentialProfile;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.TimedRobot;
+
+public class Robot extends TimedRobot {
+  private static double kDt = 0.02;
+
+  private final Joystick m_joystick = new Joystick(1);
+  private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(1);
+  // Note: These gains are fake, and will have to be tuned for your robot.
+  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(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.
+  private final ExponentialProfile m_profile =
+      new ExponentialProfile(ExponentialProfile.Constraints.fromCharacteristics(10, 1, 1));
+  private ExponentialProfile.State m_goal = new ExponentialProfile.State(0, 0);
+  private ExponentialProfile.State m_setpoint = new ExponentialProfile.State(0, 0);
+
+  public Robot() {
+    // Note: These gains are fake, and will have to be tuned for your robot.
+    m_motor.setPID(1.3, 0.0, 0.7);
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    if (m_joystick.getRawButtonPressed(2)) {
+      m_goal = new ExponentialProfile.State(5, 0);
+    } else if (m_joystick.getRawButtonPressed(3)) {
+      m_goal = new ExponentialProfile.State(0, 0);
+    }
+
+    // Retrieve the profiled setpoint for the next timestep. This setpoint moves
+    // toward the goal while obeying the constraints.
+    ExponentialProfile.State next = m_profile.calculate(kDt, m_setpoint, m_goal);
+
+    // Send setpoint to offboard controller PID
+    m_motor.setSetpoint(
+        ExampleSmartMotorController.PIDMode.kPosition,
+        m_setpoint.position,
+        m_feedforward.calculate(next.velocity) / 12.0);
+
+    m_setpoint = next;
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/elevatorexponentialsimulation/Constants.java

@@ -0,0 +1,39 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation;
+
+import edu.wpi.first.math.util.Units;
+
+public class Constants {
+  public static final int kMotorPort = 0;
+  public static final int kEncoderAChannel = 0;
+  public static final int kEncoderBChannel = 1;
+  public static final int kJoystickPort = 0;
+
+  public static final double kElevatorKp = 0.75;
+  public static final double kElevatorKi = 0;
+  public static final double kElevatorKd = 0;
+
+  public static final double kElevatorMaxV = 10.0; // volts (V)
+  public static final double kElevatorkS = 0.0; // volts (V)
+  public static final double kElevatorkG = 0.62; // volts (V)
+  public static final double kElevatorkV = 3.9; // volts (V)
+  public static final double kElevatorkA = 0.06; // volts (V)
+
+  public static final double kElevatorGearing = 5.0;
+  public static final double kElevatorDrumRadius = Units.inchesToMeters(1.0);
+  public static final double kCarriageMass = Units.lbsToKilograms(12); // kg
+
+  public static final double kSetpoint = Units.inchesToMeters(42.875);
+  public static final double kLowerkSetpoint = Units.inchesToMeters(15);
+  // Encoder is reset to measure 0 at the bottom, so minimum height is 0.
+  public static final double kMinElevatorHeight = 0.0; // m
+  public static final double kMaxElevatorHeight = Units.inchesToMeters(50);
+
+  // distance per pulse = (distance per revolution) / (pulses per revolution)
+  //  = (Pi * D) / ppr
+  public static final double kElevatorEncoderDistPerPulse =
+      2.0 * Math.PI * kElevatorDrumRadius / 4096;
+}

wpilibjExamples/src/main/java/org/wpilib/examples/elevatorexponentialsimulation/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/elevatorexponentialsimulation/Robot.java

@@ -0,0 +1,59 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation.subsystems.Elevator;
+
+/** This is a sample program to demonstrate the use of elevator simulation. */
+public class Robot extends TimedRobot {
+  private final Joystick m_joystick = new Joystick(Constants.kJoystickPort);
+  private final Elevator m_elevator = new Elevator();
+
+  public Robot() {
+    super(0.020);
+  }
+
+  @Override
+  public void robotPeriodic() {
+    // Update the telemetry, including mechanism visualization, regardless of mode.
+    m_elevator.updateTelemetry();
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    // Update the simulation model.
+    m_elevator.simulationPeriodic();
+  }
+
+  @Override
+  public void teleopInit() {
+    m_elevator.reset();
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    if (m_joystick.getTrigger()) {
+      // Here, we set the constant setpoint of 10 meters.
+      m_elevator.reachGoal(Constants.kSetpoint);
+    } else {
+      // Otherwise, we update the setpoint to 1 meter.
+      m_elevator.reachGoal(Constants.kLowerkSetpoint);
+    }
+  }
+
+  @Override
+  public void disabledInit() {
+    // This just makes sure that our simulation code knows that the motor's off.
+    m_elevator.stop();
+  }
+
+  @Override
+  public void close() {
+    m_elevator.close();
+    super.close();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/elevatorexponentialsimulation/subsystems/Elevator.java

@@ -0,0 +1,141 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation.subsystems;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.ExponentialProfile;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation.Constants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.PWMMotorControllerSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class Elevator implements AutoCloseable {
+  // This gearbox represents a gearbox containing 4 Vex 775pro motors.
+  private final DCMotor m_elevatorGearbox = DCMotor.getNEO(2);
+
+  private final ExponentialProfile m_profile =
+      new ExponentialProfile(
+          ExponentialProfile.Constraints.fromCharacteristics(
+              Constants.kElevatorMaxV, Constants.kElevatorkV, Constants.kElevatorkA));
+
+  private ExponentialProfile.State m_setpoint = new ExponentialProfile.State(0, 0);
+
+  // Standard classes for controlling our elevator
+  private final PIDController m_pidController =
+      new PIDController(Constants.kElevatorKp, Constants.kElevatorKi, Constants.kElevatorKd);
+
+  ElevatorFeedforward m_feedforward =
+      new ElevatorFeedforward(
+          Constants.kElevatorkS,
+          Constants.kElevatorkG,
+          Constants.kElevatorkV,
+          Constants.kElevatorkA);
+  private final Encoder m_encoder =
+      new Encoder(Constants.kEncoderAChannel, Constants.kEncoderBChannel);
+  private final PWMSparkMax m_motor = new PWMSparkMax(Constants.kMotorPort);
+
+  // Simulation classes help us simulate what's going on, including gravity.
+  private final ElevatorSim m_elevatorSim =
+      new ElevatorSim(
+          m_elevatorGearbox,
+          Constants.kElevatorGearing,
+          Constants.kCarriageMass,
+          Constants.kElevatorDrumRadius,
+          Constants.kMinElevatorHeight,
+          Constants.kMaxElevatorHeight,
+          true,
+          0,
+          0.005,
+          0.0);
+  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
+  private final PWMMotorControllerSim m_motorSim = new PWMMotorControllerSim(m_motor);
+
+  // Create a Mechanism2d visualization of the elevator
+  private final Mechanism2d m_mech2d =
+      new Mechanism2d(Units.inchesToMeters(10), Units.inchesToMeters(51));
+  private final MechanismRoot2d m_mech2dRoot =
+      m_mech2d.getRoot("Elevator Root", Units.inchesToMeters(5), Units.inchesToMeters(0.5));
+  private final MechanismLigament2d m_elevatorMech2d =
+      m_mech2dRoot.append(new MechanismLigament2d("Elevator", m_elevatorSim.getPosition(), 90));
+
+  /** Subsystem constructor. */
+  public Elevator() {
+    m_encoder.setDistancePerPulse(Constants.kElevatorEncoderDistPerPulse);
+
+    // Publish Mechanism2d to SmartDashboard
+    // To view the Elevator visualization, select Network Tables -> SmartDashboard -> Elevator Sim
+    SmartDashboard.putData("Elevator Sim", m_mech2d);
+  }
+
+  /** Advance the simulation. */
+  public void simulationPeriodic() {
+    // In this method, we update our simulation of what our elevator is doing
+    // First, we set our "inputs" (voltages)
+    m_elevatorSim.setInput(m_motorSim.getSpeed() * RobotController.getBatteryVoltage());
+
+    // Next, we update it. The standard loop time is 20ms.
+    m_elevatorSim.update(0.020);
+
+    // Finally, we set our simulated encoder's readings and simulated battery voltage
+    m_encoderSim.setDistance(m_elevatorSim.getPosition());
+    // SimBattery estimates loaded battery voltages
+    RoboRioSim.setVInVoltage(
+        BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDraw()));
+  }
+
+  /**
+   * Run control loop to reach and maintain goal.
+   *
+   * @param goal the position to maintain
+   */
+  public void reachGoal(double goal) {
+    var goalState = new ExponentialProfile.State(goal, 0);
+
+    var next = m_profile.calculate(0.020, m_setpoint, goalState);
+
+    // With the setpoint value we run PID control like normal
+    double pidOutput = m_pidController.calculate(m_encoder.getDistance(), m_setpoint.position);
+    double feedforwardOutput = m_feedforward.calculate(m_setpoint.velocity, next.velocity);
+
+    m_motor.setVoltage(pidOutput + feedforwardOutput);
+
+    m_setpoint = next;
+  }
+
+  /** Stop the control loop and motor output. */
+  public void stop() {
+    m_motor.set(0.0);
+  }
+
+  /** Reset Exponential profile to begin from current position on enable. */
+  public void reset() {
+    m_setpoint = new ExponentialProfile.State(m_encoder.getDistance(), 0);
+  }
+
+  /** Update telemetry, including the mechanism visualization. */
+  public void updateTelemetry() {
+    // Update elevator visualization with position
+    m_elevatorMech2d.setLength(m_encoder.getDistance());
+  }
+
+  @Override
+  public void close() {
+    m_encoder.close();
+    m_motor.close();
+    m_mech2d.close();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/elevatorprofiledpid/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorprofiledpid;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/elevatorprofiledpid/Robot.java

@@ -0,0 +1,55 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorprofiledpid;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+
+public class Robot extends TimedRobot {
+  private static double kDt = 0.02;
+  private static double kMaxVelocity = 1.75;
+  private static double kMaxAcceleration = 0.75;
+  private static double kP = 1.3;
+  private static double kI = 0.0;
+  private static double kD = 0.7;
+  private static double kS = 1.1;
+  private static double kG = 1.2;
+  private static double kV = 1.3;
+
+  private final Joystick m_joystick = new Joystick(1);
+  private final Encoder m_encoder = new Encoder(1, 2);
+  private final PWMSparkMax m_motor = new PWMSparkMax(1);
+
+  // Create a PID controller whose setpoint's change is subject to maximum
+  // velocity and acceleration constraints.
+  private final TrapezoidProfile.Constraints m_constraints =
+      new TrapezoidProfile.Constraints(kMaxVelocity, kMaxAcceleration);
+  private final ProfiledPIDController m_controller =
+      new ProfiledPIDController(kP, kI, kD, m_constraints, kDt);
+  private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward(kS, kG, kV);
+
+  public Robot() {
+    m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    if (m_joystick.getRawButtonPressed(2)) {
+      m_controller.setGoal(5);
+    } else if (m_joystick.getRawButtonPressed(3)) {
+      m_controller.setGoal(0);
+    }
+
+    // Run controller and update motor output
+    m_motor.setVoltage(
+        m_controller.calculate(m_encoder.getDistance())
+            + m_feedforward.calculate(m_controller.getSetpoint().velocity));
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/elevatorsimulation/Constants.java

@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.elevatorsimulation;
+
+import edu.wpi.first.math.util.Units;
+
+public class Constants {
+  public static final int kMotorPort = 0;
+  public static final int kEncoderAChannel = 0;
+  public static final int kEncoderBChannel = 1;
+  public static final int kJoystickPort = 0;
+
+  public static final double kElevatorKp = 5;
+  public static final double kElevatorKi = 0;
+  public static final double kElevatorKd = 0;
+
+  public static final double kElevatorkS = 0.0; // volts (V)
+  public static final double kElevatorkG = 0.762; // volts (V)
+  public static final double kElevatorkV = 0.762; // volt per velocity (V/(m/s))
+  public static final double kElevatorkA = 0.0; // volt per acceleration (V/(m/s²))
+
+  public static final double kElevatorGearing = 10.0;
+  public static final double kElevatorDrumRadius = Units.inchesToMeters(2.0);
+  public static final double kCarriageMass = 4.0; // kg
+
+  public static final double kSetpoint = 0.75; // m
+  // Encoder is reset to measure 0 at the bottom, so minimum height is 0.
+  public static final double kMinElevatorHeight = 0.0; // m
+  public static final double kMaxElevatorHeight = 1.25; // m
+
+  // distance per pulse = (distance per revolution) / (pulses per revolution)
+  //  = (Pi * D) / ppr
+  public static final double kElevatorEncoderDistPerPulse =
+      2.0 * Math.PI * kElevatorDrumRadius / 4096;
+}

wpilibjExamples/src/main/java/org/wpilib/examples/elevatorsimulation/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorsimulation;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/elevatorsimulation/Robot.java

@@ -0,0 +1,52 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorsimulation;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.examples.elevatorsimulation.subsystems.Elevator;
+
+/** This is a sample program to demonstrate the use of elevator simulation. */
+public class Robot extends TimedRobot {
+  private final Joystick m_joystick = new Joystick(Constants.kJoystickPort);
+  private final Elevator m_elevator = new Elevator();
+
+  public Robot() {}
+
+  @Override
+  public void robotPeriodic() {
+    // Update the telemetry, including mechanism visualization, regardless of mode.
+    m_elevator.updateTelemetry();
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    // Update the simulation model.
+    m_elevator.simulationPeriodic();
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    if (m_joystick.getTrigger()) {
+      // Here, we set the constant setpoint of 0.75 meters.
+      m_elevator.reachGoal(Constants.kSetpoint);
+    } else {
+      // Otherwise, we update the setpoint to 0.
+      m_elevator.reachGoal(0.0);
+    }
+  }
+
+  @Override
+  public void disabledInit() {
+    // This just makes sure that our simulation code knows that the motor's off.
+    m_elevator.stop();
+  }
+
+  @Override
+  public void close() {
+    m_elevator.close();
+    super.close();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/elevatorsimulation/subsystems/Elevator.java

@@ -0,0 +1,125 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.elevatorsimulation.subsystems;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.examples.elevatorsimulation.Constants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.PWMMotorControllerSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class Elevator implements AutoCloseable {
+  // This gearbox represents a gearbox containing 4 Vex 775pro motors.
+  private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4);
+
+  // Standard classes for controlling our elevator
+  private final ProfiledPIDController m_controller =
+      new ProfiledPIDController(
+          Constants.kElevatorKp,
+          Constants.kElevatorKi,
+          Constants.kElevatorKd,
+          new TrapezoidProfile.Constraints(2.45, 2.45));
+  ElevatorFeedforward m_feedforward =
+      new ElevatorFeedforward(
+          Constants.kElevatorkS,
+          Constants.kElevatorkG,
+          Constants.kElevatorkV,
+          Constants.kElevatorkA);
+  private final Encoder m_encoder =
+      new Encoder(Constants.kEncoderAChannel, Constants.kEncoderBChannel);
+  private final PWMSparkMax m_motor = new PWMSparkMax(Constants.kMotorPort);
+
+  // Simulation classes help us simulate what's going on, including gravity.
+  private final ElevatorSim m_elevatorSim =
+      new ElevatorSim(
+          m_elevatorGearbox,
+          Constants.kElevatorGearing,
+          Constants.kCarriageMass,
+          Constants.kElevatorDrumRadius,
+          Constants.kMinElevatorHeight,
+          Constants.kMaxElevatorHeight,
+          true,
+          0,
+          0.01,
+          0.0);
+  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
+  private final PWMMotorControllerSim m_motorSim = new PWMMotorControllerSim(m_motor);
+
+  // Create a Mechanism2d visualization of the elevator
+  private final Mechanism2d m_mech2d = new Mechanism2d(20, 50);
+  private final MechanismRoot2d m_mech2dRoot = m_mech2d.getRoot("Elevator Root", 10, 0);
+  private final MechanismLigament2d m_elevatorMech2d =
+      m_mech2dRoot.append(new MechanismLigament2d("Elevator", m_elevatorSim.getPosition(), 90));
+
+  /** Subsystem constructor. */
+  public Elevator() {
+    m_encoder.setDistancePerPulse(Constants.kElevatorEncoderDistPerPulse);
+
+    // Publish Mechanism2d to SmartDashboard
+    // To view the Elevator visualization, select Network Tables -> SmartDashboard -> Elevator Sim
+    SmartDashboard.putData("Elevator Sim", m_mech2d);
+  }
+
+  /** Advance the simulation. */
+  public void simulationPeriodic() {
+    // In this method, we update our simulation of what our elevator is doing
+    // First, we set our "inputs" (voltages)
+    m_elevatorSim.setInput(m_motorSim.getSpeed() * RobotController.getBatteryVoltage());
+
+    // Next, we update it. The standard loop time is 20ms.
+    m_elevatorSim.update(0.020);
+
+    // Finally, we set our simulated encoder's readings and simulated battery voltage
+    m_encoderSim.setDistance(m_elevatorSim.getPosition());
+    // SimBattery estimates loaded battery voltages
+    RoboRioSim.setVInVoltage(
+        BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDraw()));
+  }
+
+  /**
+   * Run control loop to reach and maintain goal.
+   *
+   * @param goal the position to maintain
+   */
+  public void reachGoal(double goal) {
+    m_controller.setGoal(goal);
+
+    // With the setpoint value we run PID control like normal
+    double pidOutput = m_controller.calculate(m_encoder.getDistance());
+    double feedforwardOutput = m_feedforward.calculate(m_controller.getSetpoint().velocity);
+    m_motor.setVoltage(pidOutput + feedforwardOutput);
+  }
+
+  /** Stop the control loop and motor output. */
+  public void stop() {
+    m_controller.setGoal(0.0);
+    m_motor.set(0.0);
+  }
+
+  /** Update telemetry, including the mechanism visualization. */
+  public void updateTelemetry() {
+    // Update elevator visualization with position
+    m_elevatorMech2d.setLength(m_encoder.getDistance());
+  }
+
+  @Override
+  public void close() {
+    m_encoder.close();
+    m_motor.close();
+    m_mech2d.close();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor controller.
+ *
+ * <p>Has no actual functionality.
+ */
+public class ExampleSmartMotorController {
+  public enum PIDMode {
+    kPosition,
+    kVelocity,
+    kMovementWitchcraft
+  }
+
+  /**
+   * Creates a new ExampleSmartMotorController.
+   *
+   * @param port The port for the controller.
+   */
+  @SuppressWarnings("PMD.UnusedFormalParameter")
+  public ExampleSmartMotorController(int port) {}
+
+  /**
+   * 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.
+   */
+  public void setPID(double kp, double ki, double kd) {}
+
+  /**
+   * 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).
+   */
+  public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
+
+  /**
+   * Places this motor controller in follower mode.
+   *
+   * @param leader The leader to follow.
+   */
+  public void follow(ExampleSmartMotorController leader) {}
+
+  /**
+   * Returns the encoder distance.
+   *
+   * @return The current encoder distance.
+   */
+  public double getEncoderDistance() {
+    return 0;
+  }
+
+  /**
+   * Returns the encoder rate.
+   *
+   * @return The current encoder rate.
+   */
+  public double getEncoderRate() {
+    return 0;
+  }
+
+  /** Resets the encoder to zero distance. */
+  public void resetEncoder() {}
+
+  public void set(double speed) {}
+
+  public double get() {
+    return 0;
+  }
+
+  public void setInverted(boolean isInverted) {}
+
+  public boolean getInverted() {
+    return false;
+  }
+
+  public void disable() {}
+
+  public void stopMotor() {}
+}

wpilibjExamples/src/main/java/org/wpilib/examples/elevatortrapezoidprofile/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/elevatortrapezoidprofile/Robot.java

@@ -0,0 +1,50 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.TimedRobot;
+
+public class Robot extends TimedRobot {
+  private static double kDt = 0.02;
+
+  private final Joystick m_joystick = new Joystick(1);
+  private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(1);
+  // Note: These gains are fake, and will have to be tuned for your robot.
+  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1.5);
+
+  // Create a motion profile with the given maximum velocity and maximum
+  // acceleration constraints for the next setpoint.
+  private final TrapezoidProfile m_profile =
+      new TrapezoidProfile(new TrapezoidProfile.Constraints(1.75, 0.75));
+  private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
+  private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
+
+  public Robot() {
+    // Note: These gains are fake, and will have to be tuned for your robot.
+    m_motor.setPID(1.3, 0.0, 0.7);
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    if (m_joystick.getRawButtonPressed(2)) {
+      m_goal = new TrapezoidProfile.State(5, 0);
+    } else if (m_joystick.getRawButtonPressed(3)) {
+      m_goal = new TrapezoidProfile.State();
+    }
+
+    // Retrieve the profiled setpoint for the next timestep. This setpoint moves
+    // toward the goal while obeying the constraints.
+    m_setpoint = m_profile.calculate(kDt, m_setpoint, m_goal);
+
+    // Send setpoint to offboard controller PID
+    m_motor.setSetpoint(
+        ExampleSmartMotorController.PIDMode.kPosition,
+        m_setpoint.position,
+        m_feedforward.calculate(m_setpoint.velocity) / 12.0);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/encoder/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.encoder;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/encoder/Robot.java

@@ -0,0 +1,70 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.encoder;
+
+import edu.wpi.first.wpilibj.CounterBase;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * 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.
+ */
+public class Robot extends TimedRobot {
+  /**
+   * The Encoder object is constructed with 4 parameters, the last two being optional. The first two
+   * parameters (1, 2 in this case) refer to the ports on the roboRIO which the encoder uses.
+   * Because a quadrature encoder has two signal wires, the signal from two DIO ports on the roboRIO
+   * are used. The third (optional) parameter is a boolean which defaults to false. If you set this
+   * parameter to true, the direction of the encoder will be reversed, in case it makes more sense
+   * mechanically. The final (optional) parameter specifies encoding rate (k1X, k2X, or k4X) and
+   * defaults to k4X. Faster (k4X) encoding gives greater positional precision but more noise in the
+   * rate.
+   */
+  private final Encoder m_encoder = new Encoder(1, 2, false, CounterBase.EncodingType.k4X);
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    /*
+     * 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.
+     */
+    m_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.
+     */
+    m_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.
+     */
+    m_encoder.setMinRate(1.0);
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    SmartDashboard.putNumber("Encoder Distance", m_encoder.getDistance());
+    SmartDashboard.putNumber("Encoder Rate", m_encoder.getRate());
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/eventloop/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.eventloop;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/eventloop/Robot.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.eventloop;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.event.BooleanEvent;
+import edu.wpi.first.wpilibj.event.EventLoop;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+
+public class Robot extends TimedRobot {
+  public static final double SHOT_VELOCITY = 200; // rpm
+  public static final double TOLERANCE = 8; // rpm
+
+  private final PWMSparkMax m_shooter = new PWMSparkMax(0);
+  private final Encoder m_shooterEncoder = new Encoder(0, 1);
+  private final PIDController m_controller = new PIDController(0.3, 0, 0);
+  private final SimpleMotorFeedforward m_ff = new SimpleMotorFeedforward(0.1, 0.065);
+
+  private final PWMSparkMax m_kicker = new PWMSparkMax(1);
+
+  private final PWMSparkMax m_intake = new PWMSparkMax(2);
+
+  private final EventLoop m_loop = new EventLoop();
+  private final Joystick m_joystick = new Joystick(0);
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    m_controller.setTolerance(TOLERANCE);
+
+    BooleanEvent isBallAtKicker =
+        new BooleanEvent(m_loop, () -> false); // m_kickerSensor.getRange() < KICKER_THRESHOLD);
+    BooleanEvent intakeButton = new BooleanEvent(m_loop, () -> m_joystick.getRawButton(2));
+
+    // if the thumb button is held
+    intakeButton
+        // and there is not a ball at the kicker
+        .and(isBallAtKicker.negate())
+        // activate the intake
+        .ifHigh(() -> m_intake.set(0.5));
+
+    // if the thumb button is not held
+    intakeButton
+        .negate()
+        // or there is a ball in the kicker
+        .or(isBallAtKicker)
+        // stop the intake
+        .ifHigh(m_intake::stopMotor);
+
+    BooleanEvent shootTrigger = new BooleanEvent(m_loop, m_joystick::getTrigger);
+
+    // if the trigger is held
+    shootTrigger
+        // accelerate the shooter wheel
+        .ifHigh(
+        () -> {
+          m_shooter.setVoltage(
+              m_controller.calculate(m_shooterEncoder.getRate(), SHOT_VELOCITY)
+                  + m_ff.calculate(SHOT_VELOCITY));
+        });
+
+    // if not, stop
+    shootTrigger.negate().ifHigh(m_shooter::stopMotor);
+
+    BooleanEvent atTargetVelocity =
+        new BooleanEvent(m_loop, m_controller::atSetpoint)
+            // debounce for more stability
+            .debounce(0.2);
+
+    // if we're at the target velocity, kick the ball into the shooter wheel
+    atTargetVelocity.ifHigh(() -> m_kicker.set(0.7));
+
+    // when we stop being at the target velocity, it means the ball was shot
+    atTargetVelocity
+        .falling()
+        // so stop the kicker
+        .ifHigh(m_kicker::stopMotor);
+  }
+
+  @Override
+  public void robotPeriodic() {
+    // poll all the bindings
+    m_loop.poll();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/examples.json

@@ -0,0 +1,761 @@
+[
+  {
+    "name": "Getting Started",
+    "description": "A differential-drive robot with split-stick Xbox arcade drive with a simple time-based autonomous.",
+    "tags": [
+      "Basic Robot"
+    ],
+    "foldername": "gettingstarted",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Tank Drive",
+    "description": "Control a differential drive with twin-joystick tank drive in teleop.",
+    "tags": [
+      "Basic Robot",
+      "Differential Drive",
+      "Joystick"
+    ],
+    "foldername": "tankdrive",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Arcade Drive",
+    "description": "Control a differential drivetrain with single-joystick arcade drive in teleop.",
+    "tags": [
+      "Basic Robot",
+      "Differential Drive",
+      "Joystick"
+    ],
+    "foldername": "arcadedrive",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Mecanum Drive",
+    "description": "Control a mecanum drivetrain with a joystick in teleop.",
+    "tags": [
+      "Basic Robot",
+      "Mecanum Drive",
+      "Joystick"
+    ],
+    "foldername": "mecanumdrive",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "PDP CAN Monitoring",
+    "description": "Monitor Power Distribution data such as voltage, current, temperature, etc.",
+    "tags": [
+      "Hardware",
+      "PDP",
+      "SmartDashboard"
+    ],
+    "foldername": "canpdp",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Solenoids",
+    "description": "Control a single and double solenoid from joystick buttons.",
+    "tags": [
+      "Hardware",
+      "Joystick",
+      "Pneumatics"
+    ],
+    "foldername": "solenoid",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Encoder",
+    "description": "View values from a quadrature encoder.",
+    "tags": [
+      "Hardware",
+      "Encoder",
+      "SmartDashboard"
+    ],
+    "foldername": "encoder",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "EventLoop",
+    "description": "Manage a ball system using EventLoop and BooleanEvent.",
+    "tags": [
+      "Basic Robot",
+      "Flywheel",
+      "EventLoop"
+    ],
+    "foldername": "eventloop",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Potentiometer PID",
+    "description": "Maintain elevator position setpoints with a potentiometer and PID control.",
+    "tags": [
+      "Basic Robot",
+      "Analog",
+      "Elevator",
+      "PID",
+      "Joystick"
+    ],
+    "foldername": "potentiometerpid",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2,
+    "hasunittests": true
+  },
+  {
+    "name": "Elevator with trapezoid profiled PID",
+    "description": "Reach elevator position setpoints with trapezoid profiles and smart motor controller PID.",
+    "tags": [
+      "Basic Robot",
+      "Elevator",
+      "Trapezoid Profile",
+      "Smart Motor Controller",
+      "Joystick"
+    ],
+    "foldername": "elevatortrapezoidprofile",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Elevator with exponential profile",
+    "description": "Reach elevator position setpoints with exponential profiles and smart motor controller PID.",
+    "tags": [
+      "Basic Robot",
+      "Elevator",
+      "Exponential Profile",
+      "Smart Motor Controller",
+      "Joystick"
+    ],
+    "foldername": "elevatorexponentialprofile",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Elevator with profiled PID controller",
+    "description": "Reach elevator position setpoints with an encoder and profiled PID control.",
+    "tags": [
+      "Basic Robot",
+      "Elevator",
+      "Profiled PID",
+      "Joystick"
+    ],
+    "foldername": "elevatorprofiledpid",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Gyro",
+    "description": "Drive a differential drive straight with a gyro sensor.",
+    "tags": [
+      "Basic Robot",
+      "Differential Drive",
+      "PID",
+      "Gyro",
+      "Analog",
+      "Joystick"
+    ],
+    "foldername": "gyro",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Gyro Mecanum",
+    "description": "Drive a mecanum drivetrain using field-oriented controls with a joystick.",
+    "tags": [
+      "Basic Robot",
+      "Mecanum Drive",
+      "Gyro",
+      "Analog",
+      "Joystick"
+    ],
+    "foldername": "gyromecanum",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "HID Rumble",
+    "description": "Make human interface devices (HID) rumble.",
+    "tags": [
+      "Hardware",
+      "XboxController"
+    ],
+    "foldername": "hidrumble",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Mechanism2d",
+    "description": "Display mechanism states on a dashboard with Mechanism2d.",
+    "tags": [
+      "Basic Robot",
+      "Elevator",
+      "Arm",
+      "Analog",
+      "Joystick",
+      "SmartDashboard",
+      "Mechanism2d"
+    ],
+    "foldername": "mechanism2d",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Motor Control",
+    "description": "Control a single motor with a joystick, displaying the movement of the motor using an encoder.",
+    "tags": [
+      "Basic Robot",
+      "Encoder",
+      "SmartDashboard",
+      "Joystick"
+    ],
+    "foldername": "motorcontrol",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Simple Vision",
+    "description": "Use the CameraServer class to stream from a USB Webcam without processing the images.",
+    "tags": [
+      "Vision"
+    ],
+    "foldername": "quickvision",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Intermediate Vision",
+    "description": "Acquire images from an attached USB camera and add some annotation to the image (as you might do for showing operators the result of some image recognition) and send it to the dashboard for display.",
+    "tags": [
+      "Vision"
+    ],
+    "foldername": "intermediatevision",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "HTTP Camera",
+    "description": "Acquire images from an HTTP network camera and adds some annotation to the image (as you might do for showing operators the result of some image recognition), and sends it to the dashboard for display.",
+    "tags": [
+      "Vision"
+    ],
+    "foldername": "httpcamera",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "AprilTags Vision",
+    "description": "On-roboRIO detection of AprilTags using an attached USB camera.",
+    "tags": [
+      "Vision",
+      "AprilTags"
+    ],
+    "foldername": "apriltagsvision",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "'Traditional' Hatchbot",
+    "description": "A fully-functional command-based hatchbot for the 2019 game, written in the 'traditional' style, i.e. commands are given their own classes.",
+    "tags": [
+      "Complete Robot",
+      "Command-based",
+      "Differential Drive",
+      "Encoder",
+      "Pneumatics",
+      "Sendable",
+      "DataLog",
+      "XboxController"
+    ],
+    "foldername": "hatchbottraditional",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "'Inlined' Hatchbot",
+    "description": "A fully-functional command-based hatchbot for the 2019 game, written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
+    "tags": [
+      "Complete Robot",
+      "Command-based",
+      "Differential Drive",
+      "Encoder",
+      "Pneumatics",
+      "Sendable",
+      "DataLog",
+      "PS4Controller"
+    ],
+    "foldername": "hatchbotinlined",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Rapid React Command Bot",
+    "description": "A fully-functional command-based fender bot for the 2022 game, written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
+    "tags": [
+      "Complete Robot",
+      "Command-based",
+      "Differential Drive",
+      "Intake",
+      "Flywheel",
+      "Encoder",
+      "Pneumatics",
+      "Digital Input",
+      "PID",
+      "Gyro",
+      "Profiled PID",
+      "XboxController"
+    ],
+    "foldername": "rapidreactcommandbot",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Select Command Example",
+    "description": "Use SelectCommand to select an autonomous routine.",
+    "tags": [
+      "Command-based"
+    ],
+    "foldername": "selectcommand",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "SwerveBot",
+    "description": "Use kinematics and odometry with a swerve drive.",
+    "tags": [
+      "Swerve Drive",
+      "Odometry",
+      "XboxController",
+      "Gyro",
+      "Encoder"
+    ],
+    "foldername": "swervebot",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "MecanumBot",
+    "description": "Use kinematics and odometry with a mecanum drive.",
+    "tags": [
+      "Mecanum Drive",
+      "Odometry",
+      "Encoder",
+      "Gyro",
+      "XboxController"
+    ],
+    "foldername": "mecanumbot",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "DifferentialDriveBot",
+    "description": "Use kinematics and odometry with a differential drive.",
+    "tags": [
+      "Differential Drive",
+      "Odometry",
+      "Encoder",
+      "Gyro",
+      "XboxController"
+    ],
+    "foldername": "differentialdrivebot",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Arcade Drive Xbox Controller",
+    "description": "Control a differential drive with split-stick arcade drive in teleop.",
+    "tags": [
+      "Basic Robot",
+      "Differential Drive",
+      "XboxController"
+    ],
+    "foldername": "arcadedrivexboxcontroller",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Tank Drive Xbox Controller",
+    "description": "Control a differential drive with Xbox tank drive in teleop.",
+    "tags": [
+      "Basic Robot",
+      "Differential Drive",
+      "XboxController"
+    ],
+    "foldername": "tankdrivexboxcontroller",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Duty Cycle Encoder",
+    "description": "View values from a duty-cycle encoder.",
+    "tags": [
+      "Hardware",
+      "Duty Cycle",
+      "Encoder",
+      "SmartDashboard"
+    ],
+    "foldername": "dutycycleencoder",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Duty Cycle Input",
+    "description": "View duty-cycle input.",
+    "tags": [
+      "Hardware",
+      "Duty Cycle",
+      "SmartDashboard"
+    ],
+    "foldername": "dutycycleinput",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Addressable LED",
+    "description": "Display a rainbow pattern on an addressable LED strip.",
+    "tags": [
+      "Hardware",
+      "Basic Robot",
+      "AddressableLEDs"
+    ],
+    "foldername": "addressableled",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "DriveDistanceOffboard",
+    "description": "Drive a differential drivetrain a set distance using TrapezoidProfile and smart motor controller PID.",
+    "tags": [
+      "Command-based",
+      "Differential Drive",
+      "Trapezoid Profile",
+      "Smart Motor Controller",
+      "XboxController"
+    ],
+    "foldername": "drivedistanceoffboard",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "StateSpaceFlywheel",
+    "description": "Control a flywheel using a state-space model (based on values from CAD), with a Kalman Filter and LQR.",
+    "tags": [
+      "Basic Robot",
+      "Flywheel",
+      "State-Space",
+      "LQR",
+      "Encoder",
+      "Joystick"
+    ],
+    "foldername": "statespaceflywheel",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "StateSpaceFlywheelSysId",
+    "description": "Control a flywheel using a state-space model (based on values from SysId), with a Kalman Filter and LQR.",
+    "tags": [
+      "Basic Robot",
+      "Flywheel",
+      "SysId",
+      "State-Space",
+      "LQR",
+      "Encoder",
+      "Joystick"
+    ],
+    "foldername": "statespaceflywheelsysid",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "StateSpaceElevator",
+    "description": "Control an elevator using a state-space model (based on values from CAD), with a Kalman Filter and LQR.",
+    "tags": [
+      "Basic Robot",
+      "Elevator",
+      "State-Space",
+      "LQR",
+      "Encoder",
+      "Joystick"
+    ],
+    "foldername": "statespaceelevator",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "StateSpaceArm",
+    "description": "Control an arm using a state-space model (based on values from CAD), with a Kalman Filter and LQR.",
+    "tags": [
+      "Basic Robot",
+      "Arm",
+      "State-Space",
+      "LQR",
+      "Encoder",
+      "Joystick"
+    ],
+    "foldername": "statespacearm",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "SimpleDifferentialDriveSimulation",
+    "description": "Simulate a differential drivetrain and follow trajectories with LTVUnicycleController (non-command-based).",
+    "tags": [
+      "Differential Drive",
+      "State-Space",
+      "LTVUnicycleController",
+      "Path Following",
+      "Trajectory",
+      "Encoder",
+      "XboxController",
+      "Simulation"
+    ],
+    "foldername": "simpledifferentialdrivesimulation",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "ElevatorSimulation",
+    "description": "Simulate an elevator.",
+    "tags": [
+      "Basic Robot",
+      "Elevator",
+      "State-Space",
+      "Simulation",
+      "Mechanism2d",
+      "Profiled PID"
+    ],
+    "foldername": "elevatorsimulation",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2,
+    "hasunittests": true
+  },
+  {
+    "name": "Elevator Exponential Profile Simulation",
+    "description": "Simulate an elevator.",
+    "tags": [
+      "Basic Robot",
+      "Elevator",
+      "State-Space",
+      "Simulation",
+      "Mechanism2d",
+      "PID",
+      "Exponential Profile"
+    ],
+    "foldername": "elevatorexponentialsimulation",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "ArmSimulation",
+    "description": "Simulate a single-jointed arm.",
+    "tags": [
+      "Basic Robot",
+      "Arm",
+      "State-Space",
+      "Simulation",
+      "Mechanism2d",
+      "Preferences"
+    ],
+    "foldername": "armsimulation",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2,
+    "hasunittests": true
+  },
+  {
+    "name": "UnitTesting",
+    "description": "Test a robot project with basic unit tests in simulation.",
+    "tags": [
+      "Intake",
+      "Pneumatics"
+    ],
+    "foldername": "unittest",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2,
+    "hasunittests": true
+  },
+  {
+    "name": "DifferentialDrivePoseEstimator",
+    "description": "Combine differential-drive odometry with vision data using DifferentialDrivePoseEstimator.",
+    "tags": [
+      "Differential Drive",
+      "State-Space",
+      "Pose Estimator",
+      "Vision",
+      "PID",
+      "XboxController"
+    ],
+    "foldername": "differentialdriveposeestimator",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "MecanumDrivePoseEstimator",
+    "description": "Combine mecanum-drive odometry with vision data using MecanumDrivePoseEstimator.",
+    "tags": [
+      "Mecanum Drive",
+      "State-Space",
+      "Pose Estimator",
+      "Vision",
+      "PID",
+      "XboxController"
+    ],
+    "foldername": "mecanumdriveposeestimator",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "SwerveDrivePoseEstimator",
+    "description": "Combine swerve-drive odometry with vision data using SwerveDrivePoseEstimator.",
+    "tags": [
+      "Swerve Drive",
+      "State-Space",
+      "Pose Estimator",
+      "Vision",
+      "PID",
+      "XboxController"
+    ],
+    "foldername": "swervedriveposeestimator",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "RomiReference",
+    "description": "An example command-based robot program that can be used with the Romi reference robot design.",
+    "tags": [
+      "Romi",
+      "Command-based",
+      "Differential Drive",
+      "Digital Input",
+      "Joystick"
+    ],
+    "foldername": "romireference",
+    "gradlebase": "javaromi",
+    "mainclass": "Main",
+    "commandversion": 2,
+    "extravendordeps": [
+      "romi"
+    ]
+  },
+  {
+    "name": "XRP Reference",
+    "description": "An example command-based robot program that can be used with the XRP reference robot design.",
+    "tags": [
+      "XRP",
+      "Command-based",
+      "Differential Drive",
+      "Digital Input",
+      "Joystick"
+    ],
+    "foldername": "xrpreference",
+    "gradlebase": "javaxrp",
+    "mainclass": "Main",
+    "commandversion": 2,
+    "extravendordeps": [
+      "xrp"
+    ]
+  },
+  {
+    "name": "Digital Communication Sample",
+    "description": "Communicates with external devices (such as an Arduino) using the roboRIO's DIO.",
+    "tags": [
+      "Hardware",
+      "Digital Output"
+    ],
+    "foldername": "digitalcommunication",
+    "gradlebase": "java",
+    "commandversion": 2,
+    "mainclass": "Main",
+    "hasunittests": true
+  },
+  {
+    "name": "I2C Communication Sample",
+    "description": "Communicate with external devices (such as an Arduino) using the roboRIO's I2C port.",
+    "tags": [
+      "Hardware",
+      "I2C"
+    ],
+    "foldername": "i2ccommunication",
+    "gradlebase": "java",
+    "commandversion": 2,
+    "mainclass": "Main",
+    "hasunittests": true
+  },
+  {
+    "name": "Flywheel BangBangController",
+    "description": "A sample program to demonstrate the use of a BangBangController with a flywheel to control RPM",
+    "tags": [
+      "Flywheel",
+      "Simulation",
+      "Joystick"
+    ],
+    "foldername": "flywheelbangbangcontroller",
+    "gradlebase": "java",
+    "commandversion": 2,
+    "mainclass": "Main"
+  },
+  {
+    "name": "SysIdRoutine",
+    "description": "A sample command-based robot demonstrating use of the SysIdRoutine command factory",
+    "tags": [
+      "SysId",
+      "Command-based",
+      "DataLog"
+    ],
+    "foldername": "sysidroutine",
+    "gradlebase": "java",
+    "commandversion": 2,
+    "mainclass": "Main"
+  }
+]

wpilibjExamples/src/main/java/org/wpilib/examples/flywheelbangbangcontroller/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.flywheelbangbangcontroller;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/flywheelbangbangcontroller/Robot.java

@@ -0,0 +1,101 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.flywheelbangbangcontroller;
+
+import edu.wpi.first.math.controller.BangBangController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * This is a sample program to demonstrate the use of a BangBangController with a flywheel to
+ * control RPM.
+ */
+public class Robot extends TimedRobot {
+  private static final int kMotorPort = 0;
+  private static final int kEncoderAChannel = 0;
+  private static final int kEncoderBChannel = 1;
+
+  // Max setpoint for joystick control in RPM
+  private static final double kMaxSetpointValue = 6000.0;
+
+  // Joystick to control setpoint
+  private final Joystick m_joystick = new Joystick(0);
+
+  private final PWMSparkMax m_flywheelMotor = new PWMSparkMax(kMotorPort);
+  private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
+
+  private final BangBangController m_bangBangController = new BangBangController();
+
+  // Gains are for example purposes only - must be determined for your own robot!
+  public static final double kFlywheelKs = 0.0001; // V
+  public static final double kFlywheelKv = 0.000195; // V/RPM
+  public static final double kFlywheelKa = 0.0003; // V/(RPM/s)
+  private final SimpleMotorFeedforward m_feedforward =
+      new SimpleMotorFeedforward(kFlywheelKs, kFlywheelKv, kFlywheelKa);
+
+  // Simulation classes help us simulate our robot
+
+  // Reduction between motors and encoder, as output over input. If the flywheel
+  // spins slower than the motors, this number should be greater than one.
+  private static final double kFlywheelGearing = 1.0;
+
+  // 1/2 MR²
+  private static final double kFlywheelMomentOfInertia =
+      0.5 * Units.lbsToKilograms(1.5) * Math.pow(Units.inchesToMeters(4), 2);
+
+  private final DCMotor m_gearbox = DCMotor.getNEO(1);
+
+  private final LinearSystem<N1, N1, N1> m_plant =
+      LinearSystemId.createFlywheelSystem(m_gearbox, kFlywheelGearing, kFlywheelMomentOfInertia);
+
+  private final FlywheelSim m_flywheelSim = new FlywheelSim(m_plant, m_gearbox);
+  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
+
+  public Robot() {
+    // Add bang-bang controller to SmartDashboard and networktables.
+    SmartDashboard.putData(m_bangBangController);
+  }
+
+  /** Controls flywheel to a set speed (RPM) controlled by a joystick. */
+  @Override
+  public void teleopPeriodic() {
+    // Scale setpoint value between 0 and maxSetpointValue
+    double setpoint =
+        Math.max(
+            0.0,
+            m_joystick.getRawAxis(0)
+                * Units.rotationsPerMinuteToRadiansPerSecond(kMaxSetpointValue));
+
+    // Set setpoint and measurement of the bang-bang controller
+    double bangOutput = m_bangBangController.calculate(m_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.
+    m_flywheelMotor.setVoltage(bangOutput + 0.9 * m_feedforward.calculate(setpoint));
+  }
+
+  /** Update our simulation. This should be run every robot loop in simulation. */
+  @Override
+  public void simulationPeriodic() {
+    // To update our simulation, we set motor voltage inputs, update the
+    // simulation, and write the simulated velocities to our simulated encoder
+    m_flywheelSim.setInputVoltage(m_flywheelMotor.get() * RobotController.getInputVoltage());
+    m_flywheelSim.update(0.02);
+    m_encoderSim.setRate(m_flywheelSim.getAngularVelocity());
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/gettingstarted/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.gettingstarted;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/gettingstarted/Robot.java

@@ -0,0 +1,73 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.gettingstarted;
+
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+
+/**
+ * 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 manifest file in the resource directory.
+ */
+public class Robot extends TimedRobot {
+  private final PWMSparkMax m_leftDrive = new PWMSparkMax(0);
+  private final PWMSparkMax m_rightDrive = new PWMSparkMax(1);
+  private final DifferentialDrive m_robotDrive =
+      new DifferentialDrive(m_leftDrive::set, m_rightDrive::set);
+  private final XboxController m_controller = new XboxController(0);
+  private final Timer m_timer = new Timer();
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    SendableRegistry.addChild(m_robotDrive, m_leftDrive);
+    SendableRegistry.addChild(m_robotDrive, m_rightDrive);
+
+    // 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.
+    m_rightDrive.setInverted(true);
+  }
+
+  /** This function is run once each time the robot enters autonomous mode. */
+  @Override
+  public void autonomousInit() {
+    m_timer.restart();
+  }
+
+  /** This function is called periodically during autonomous. */
+  @Override
+  public void autonomousPeriodic() {
+    // Drive for 2 seconds
+    if (m_timer.get() < 2.0) {
+      // Drive forwards half speed, make sure to turn input squaring off
+      m_robotDrive.arcadeDrive(0.5, 0.0, false);
+    } else {
+      m_robotDrive.stopMotor(); // stop robot
+    }
+  }
+
+  /** This function is called once each time the robot enters teleoperated mode. */
+  @Override
+  public void teleopInit() {}
+
+  /** This function is called periodically during teleoperated mode. */
+  @Override
+  public void teleopPeriodic() {
+    m_robotDrive.arcadeDrive(-m_controller.getLeftY(), -m_controller.getRightX());
+  }
+
+  /** This function is called once each time the robot enters test mode. */
+  @Override
+  public void testInit() {}
+
+  /** This function is called periodically during test mode. */
+  @Override
+  public void testPeriodic() {}
+}

wpilibjExamples/src/main/java/org/wpilib/examples/gyro/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.gyro;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/gyro/Robot.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.gyro;
+
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.OnboardIMU;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+
+/**
+ * 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.
+ */
+public class Robot extends TimedRobot {
+  private static final double kAngleSetpoint = 0.0;
+  private static final double kP = 0.005; // proportional turning constant
+
+  private static final int kLeftMotorPort = 0;
+  private static final int kRightMotorPort = 1;
+  private static final OnboardIMU.MountOrientation kIMUMountOrientation =
+      OnboardIMU.MountOrientation.kFlat;
+  private static final int kJoystickPort = 0;
+
+  private final PWMSparkMax m_leftDrive = new PWMSparkMax(kLeftMotorPort);
+  private final PWMSparkMax m_rightDrive = new PWMSparkMax(kRightMotorPort);
+  private final DifferentialDrive m_robotDrive =
+      new DifferentialDrive(m_leftDrive::set, m_rightDrive::set);
+  private final OnboardIMU m_imu = new OnboardIMU(kIMUMountOrientation);
+  private final Joystick m_joystick = new Joystick(kJoystickPort);
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    SendableRegistry.addChild(m_robotDrive, m_leftDrive);
+    SendableRegistry.addChild(m_robotDrive, m_rightDrive);
+
+    // 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.
+    m_rightDrive.setInverted(true);
+  }
+
+  /**
+   * 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.
+   */
+  @Override
+  public void teleopPeriodic() {
+    double turningValue = (kAngleSetpoint - m_imu.getRotation2d().getDegrees()) * kP;
+    m_robotDrive.arcadeDrive(-m_joystick.getY(), -turningValue);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/gyromecanum/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.gyromecanum;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/gyromecanum/Robot.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.gyromecanum;
+
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.OnboardIMU;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.drive.MecanumDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+
+/**
+ * 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).
+ */
+public class Robot extends TimedRobot {
+  private static final int kFrontLeftChannel = 0;
+  private static final int kRearLeftChannel = 1;
+  private static final int kFrontRightChannel = 2;
+  private static final int kRearRightChannel = 3;
+  private static final OnboardIMU.MountOrientation kIMUMountOrientation =
+      OnboardIMU.MountOrientation.kFlat;
+  private static final int kJoystickPort = 0;
+
+  private final MecanumDrive m_robotDrive;
+  private final OnboardIMU m_imu = new OnboardIMU(kIMUMountOrientation);
+  private final Joystick m_joystick = new Joystick(kJoystickPort);
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    PWMSparkMax frontLeft = new PWMSparkMax(kFrontLeftChannel);
+    PWMSparkMax rearLeft = new PWMSparkMax(kRearLeftChannel);
+    PWMSparkMax frontRight = new PWMSparkMax(kFrontRightChannel);
+    PWMSparkMax rearRight = new PWMSparkMax(kRearRightChannel);
+
+    // Invert the right side motors.
+    // You may need to change or remove this to match your robot.
+    frontRight.setInverted(true);
+    rearRight.setInverted(true);
+
+    m_robotDrive = new MecanumDrive(frontLeft::set, rearLeft::set, frontRight::set, rearRight::set);
+
+    SendableRegistry.addChild(m_robotDrive, frontLeft);
+    SendableRegistry.addChild(m_robotDrive, rearLeft);
+    SendableRegistry.addChild(m_robotDrive, frontRight);
+    SendableRegistry.addChild(m_robotDrive, rearRight);
+  }
+
+  /** Mecanum drive is used with the gyro angle as an input. */
+  @Override
+  public void teleopPeriodic() {
+    m_robotDrive.driveCartesian(
+        -m_joystick.getY(), -m_joystick.getX(), -m_joystick.getZ(), m_imu.getRotation2d());
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotinlined/Constants.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+  public static final class DriveConstants {
+    public static final int kLeftMotor1Port = 0;
+    public static final int kLeftMotor2Port = 1;
+    public static final int kRightMotor1Port = 2;
+    public static final int kRightMotor2Port = 3;
+
+    public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+    public static final int[] kRightEncoderPorts = new int[] {2, 3};
+    public static final boolean kLeftEncoderReversed = false;
+    public static final boolean kRightEncoderReversed = true;
+
+    public static final int kEncoderCPR = 1024;
+    public static final double kWheelDiameterInches = 6;
+    public static final double kEncoderDistancePerPulse =
+        // Assumes the encoders are directly mounted on the wheel shafts
+        (kWheelDiameterInches * Math.PI) / kEncoderCPR;
+  }
+
+  public static final class HatchConstants {
+    public static final int kHatchSolenoidModule = 0;
+    public static final int[] kHatchSolenoidPorts = new int[] {0, 1};
+  }
+
+  public static final class AutoConstants {
+    public static final double kAutoDriveDistanceInches = 60;
+    public static final double kAutoBackupDistanceInches = 20;
+    public static final double kAutoDriveSpeed = 0.5;
+  }
+
+  public static final class OIConstants {
+    public static final int kDriverControllerPort = 0;
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotinlined/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotinlined/Robot.java

@@ -0,0 +1,102 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined;
+
+import edu.wpi.first.wpilibj.DataLogManager;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * 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.
+ */
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+
+  private final RobotContainer m_robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  public Robot() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+
+    // Start recording to data log
+    DataLogManager.start();
+
+    // Record DS control and joystick data.
+    // Change to `false` to not record joystick data.
+    DriverStation.startDataLog(DataLogManager.getLog(), true);
+  }
+
+  /**
+   * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
+   * that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /** This function is called once each time the robot enters Disabled mode. */
+  @Override
+  public void disabledInit() {}
+
+  @Override
+  public void disabledPeriodic() {}
+
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null) {
+      CommandScheduler.getInstance().schedule(m_autonomousCommand);
+    }
+  }
+
+  /** This function is called periodically during autonomous. */
+  @Override
+  public void autonomousPeriodic() {}
+
+  @Override
+  public void teleopInit() {
+    // 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 (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  /** This function is called periodically during operator control. */
+  @Override
+  public void teleopPeriodic() {}
+
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  /** This function is called periodically during test mode. */
+  @Override
+  public void testPeriodic() {}
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotinlined/RobotContainer.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined;
+
+import edu.wpi.first.wpilibj.PS4Controller;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.OIConstants;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.commands.Autos;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
+
+/**
+ * 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 {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+  // The robot's subsystems
+  private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+  private final HatchSubsystem m_hatchSubsystem = new HatchSubsystem();
+
+  // Retained command handles
+
+  // The autonomous routines
+  // A simple auto routine that drives forward a specified distance, and then stops.
+  private final Command m_simpleAuto = Autos.simpleAuto(m_robotDrive);
+  // A complex auto routine that drives forward, drops a hatch, and then drives backward.
+  private final Command m_complexAuto = Autos.complexAuto(m_robotDrive, m_hatchSubsystem);
+
+  // A chooser for autonomous commands
+  SendableChooser<Command> m_chooser = new SendableChooser<>();
+
+  // The driver's controller
+  CommandPS4Controller m_driverController =
+      new CommandPS4Controller(OIConstants.kDriverControllerPort);
+
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
+  public RobotContainer() {
+    // Configure the button bindings
+    configureButtonBindings();
+
+    // Configure default commands
+    // Set the default drive command to split-stick arcade drive
+    m_robotDrive.setDefaultCommand(
+        // A split-stick arcade command, with forward/backward controlled by the left
+        // hand, and turning controlled by the right.
+        Commands.run(
+            () ->
+                m_robotDrive.arcadeDrive(
+                    -m_driverController.getLeftY(), -m_driverController.getRightX()),
+            m_robotDrive));
+
+    // Add commands to the autonomous command chooser
+    m_chooser.setDefaultOption("Simple Auto", m_simpleAuto);
+    m_chooser.addOption("Complex Auto", m_complexAuto);
+
+    // Put the chooser on the dashboard
+    SmartDashboard.putData("Autonomous", m_chooser);
+
+    // Put subsystems to dashboard.
+    SmartDashboard.putData("Drivetrain", m_robotDrive);
+    SmartDashboard.putData("HatchSubsystem", m_hatchSubsystem);
+  }
+
+  /**
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link PS4Controller}), and then passing it to a {@link
+   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   */
+  private void configureButtonBindings() {
+    // Grab the hatch when the Circle button is pressed.
+    m_driverController.circle().onTrue(m_hatchSubsystem.grabHatchCommand());
+    // Release the hatch when the Square button is pressed.
+    m_driverController.square().onTrue(m_hatchSubsystem.releaseHatchCommand());
+    // While holding R1, drive at half speed
+    m_driverController
+        .R1()
+        .onTrue(Commands.runOnce(() -> m_robotDrive.setMaxOutput(0.5)))
+        .onFalse(Commands.runOnce(() -> m_robotDrive.setMaxOutput(1)));
+  }
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    return m_chooser.getSelected();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotinlined/commands/Autos.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined.commands;
+
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+
+/** Container for auto command factories. */
+public final class Autos {
+  /** A simple auto routine that drives forward a specified distance, and then stops. */
+  public static Command simpleAuto(DriveSubsystem drive) {
+    return new FunctionalCommand(
+        // Reset encoders on command start
+        drive::resetEncoders,
+        // Drive forward while the command is executing
+        () -> drive.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0),
+        // Stop driving at the end of the command
+        interrupt -> drive.arcadeDrive(0, 0),
+        // End the command when the robot's driven distance exceeds the desired value
+        () -> drive.getAverageEncoderDistance() >= AutoConstants.kAutoDriveDistanceInches,
+        // Require the drive subsystem
+        drive);
+  }
+
+  /** A complex auto routine that drives forward, drops a hatch, and then drives backward. */
+  public static Command complexAuto(DriveSubsystem driveSubsystem, HatchSubsystem hatchSubsystem) {
+    return Commands.sequence(
+        // Drive forward up to the front of the cargo ship
+        new FunctionalCommand(
+            // Reset encoders on command start
+            driveSubsystem::resetEncoders,
+            // Drive forward while the command is executing
+            () -> driveSubsystem.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0),
+            // Stop driving at the end of the command
+            interrupt -> driveSubsystem.arcadeDrive(0, 0),
+            // End the command when the robot's driven distance exceeds the desired value
+            () ->
+                driveSubsystem.getAverageEncoderDistance()
+                    >= AutoConstants.kAutoDriveDistanceInches,
+            // Require the drive subsystem
+            driveSubsystem),
+
+        // Release the hatch
+        hatchSubsystem.releaseHatchCommand(),
+
+        // Drive backward the specified distance
+        new FunctionalCommand(
+            // Reset encoders on command start
+            driveSubsystem::resetEncoders,
+            // Drive backward while the command is executing
+            () -> driveSubsystem.arcadeDrive(-AutoConstants.kAutoDriveSpeed, 0),
+            // Stop driving at the end of the command
+            interrupt -> driveSubsystem.arcadeDrive(0, 0),
+            // End the command when the robot's driven distance exceeds the desired value
+            () ->
+                driveSubsystem.getAverageEncoderDistance()
+                    <= AutoConstants.kAutoBackupDistanceInches,
+            // Require the drive subsystem
+            driveSubsystem));
+  }
+
+  private Autos() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotinlined/subsystems/DriveSubsystem.java

@@ -0,0 +1,101 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class DriveSubsystem extends SubsystemBase {
+  // The motors on the left side of the drive.
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
+
+  // The motors on the right side of the drive.
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
+
+  // The robot's drive
+  private final DifferentialDrive m_drive =
+      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
+
+  // The left-side drive encoder
+  private final Encoder m_leftEncoder =
+      new Encoder(
+          DriveConstants.kLeftEncoderPorts[0],
+          DriveConstants.kLeftEncoderPorts[1],
+          DriveConstants.kLeftEncoderReversed);
+
+  // The right-side drive encoder
+  private final Encoder m_rightEncoder =
+      new Encoder(
+          DriveConstants.kRightEncoderPorts[0],
+          DriveConstants.kRightEncoderPorts[1],
+          DriveConstants.kRightEncoderReversed);
+
+  /** Creates a new DriveSubsystem. */
+  public DriveSubsystem() {
+    SendableRegistry.addChild(m_drive, m_leftLeader);
+    SendableRegistry.addChild(m_drive, m_rightLeader);
+
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_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.
+    m_rightLeader.setInverted(true);
+
+    // Sets the distance per pulse for the encoders
+    m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+    m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+  }
+
+  /**
+   * Drives the robot using arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  public void arcadeDrive(double fwd, double rot) {
+    m_drive.arcadeDrive(fwd, rot);
+  }
+
+  /** Resets the drive encoders to currently read a position of 0. */
+  public void resetEncoders() {
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  /**
+   * Gets the average distance of the TWO encoders.
+   *
+   * @return the average of the TWO encoder readings
+   */
+  public double getAverageEncoderDistance() {
+    return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
+  }
+
+  /**
+   * 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
+   */
+  public void setMaxOutput(double maxOutput) {
+    m_drive.setMaxOutput(maxOutput);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+    // Publish encoder distances to telemetry.
+    builder.addDoubleProperty("leftDistance", m_leftEncoder::getDistance, null);
+    builder.addDoubleProperty("rightDistance", m_rightEncoder::getDistance, null);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotinlined/subsystems/HatchSubsystem.java

@@ -0,0 +1,44 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems;
+
+import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
+import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/** A hatch mechanism actuated by a single {@link edu.wpi.first.wpilibj.DoubleSolenoid}. */
+public class HatchSubsystem extends SubsystemBase {
+  private final DoubleSolenoid m_hatchSolenoid =
+      new DoubleSolenoid(
+          0,
+          PneumaticsModuleType.CTREPCM,
+          HatchConstants.kHatchSolenoidPorts[0],
+          HatchConstants.kHatchSolenoidPorts[1]);
+
+  /** Grabs the hatch. */
+  public Command grabHatchCommand() {
+    // implicitly require `this`
+    return this.runOnce(() -> m_hatchSolenoid.set(kForward));
+  }
+
+  /** Releases the hatch. */
+  public Command releaseHatchCommand() {
+    // implicitly require `this`
+    return this.runOnce(() -> m_hatchSolenoid.set(kReverse));
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+    // Publish the solenoid state to telemetry.
+    builder.addBooleanProperty("extended", () -> m_hatchSolenoid.get() == kForward, null);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/Constants.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+  public static final class DriveConstants {
+    public static final int kLeftMotor1Port = 0;
+    public static final int kLeftMotor2Port = 1;
+    public static final int kRightMotor1Port = 2;
+    public static final int kRightMotor2Port = 3;
+
+    public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+    public static final int[] kRightEncoderPorts = new int[] {2, 3};
+    public static final boolean kLeftEncoderReversed = false;
+    public static final boolean kRightEncoderReversed = true;
+
+    public static final int kEncoderCPR = 1024;
+    public static final double kWheelDiameterInches = 6;
+    public static final double kEncoderDistancePerPulse =
+        // Assumes the encoders are directly mounted on the wheel shafts
+        (kWheelDiameterInches * Math.PI) / kEncoderCPR;
+  }
+
+  public static final class HatchConstants {
+    public static final int kHatchSolenoidModule = 0;
+    public static final int[] kHatchSolenoidPorts = new int[] {0, 1};
+  }
+
+  public static final class AutoConstants {
+    public static final double kAutoDriveDistanceInches = 60;
+    public static final double kAutoBackupDistanceInches = 20;
+    public static final double kAutoDriveSpeed = 0.5;
+  }
+
+  public static final class OIConstants {
+    public static final int kDriverControllerPort = 0;
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/Robot.java

@@ -0,0 +1,109 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional;
+
+import edu.wpi.first.wpilibj.DataLogManager;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * 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.
+ */
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+
+  private final RobotContainer m_robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  public Robot() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+
+    // Start recording to data log
+    DataLogManager.start();
+
+    // Record DS control and joystick data.
+    // Change to `false` to not record joystick data.
+    DriverStation.startDataLog(DataLogManager.getLog(), true);
+  }
+
+  /**
+   * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
+   * that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /** This function is called once each time the robot enters Disabled mode. */
+  @Override
+  public void disabledInit() {}
+
+  @Override
+  public void disabledPeriodic() {}
+
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    /*
+     * String autoSelected = SmartDashboard.getString("Auto Selector",
+     * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+     * = new MyAutoCommand(); break; case "Default Auto": default:
+     * autonomousCommand = new ExampleCommand(); break; }
+     */
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null) {
+      CommandScheduler.getInstance().schedule(m_autonomousCommand);
+    }
+  }
+
+  /** This function is called periodically during autonomous. */
+  @Override
+  public void autonomousPeriodic() {}
+
+  @Override
+  public void teleopInit() {
+    // 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 (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  /** This function is called periodically during operator control. */
+  @Override
+  public void teleopPeriodic() {}
+
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  /** This function is called periodically during test mode. */
+  @Override
+  public void testPeriodic() {}
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/RobotContainer.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.OIConstants;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.ComplexAuto;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.DefaultDrive;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.DriveDistance;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.GrabHatch;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.HalveDriveSpeed;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.ReleaseHatch;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+/**
+ * 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 {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+  // The robot's subsystems
+  private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+  private final HatchSubsystem m_hatchSubsystem = new HatchSubsystem();
+
+  // The autonomous routines
+
+  // A simple auto routine that drives forward a specified distance, and then stops.
+  private final Command m_simpleAuto =
+      new DriveDistance(
+          AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed, m_robotDrive);
+
+  // A complex auto routine that drives forward, drops a hatch, and then drives backward.
+  private final Command m_complexAuto = new ComplexAuto(m_robotDrive, m_hatchSubsystem);
+
+  // A chooser for autonomous commands
+  SendableChooser<Command> m_chooser = new SendableChooser<>();
+
+  // The driver's controller
+  XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
+
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
+  public RobotContainer() {
+    // Configure the button bindings
+    configureButtonBindings();
+
+    // Configure default commands
+    // Set the default drive command to split-stick arcade drive
+    m_robotDrive.setDefaultCommand(
+        // A split-stick arcade command, with forward/backward controlled by the left
+        // hand, and turning controlled by the right.
+        new DefaultDrive(
+            m_robotDrive,
+            () -> -m_driverController.getLeftY(),
+            () -> -m_driverController.getRightX()));
+
+    // Add commands to the autonomous command chooser
+    m_chooser.setDefaultOption("Simple Auto", m_simpleAuto);
+    m_chooser.addOption("Complex Auto", m_complexAuto);
+
+    // Put the chooser on the dashboard
+    SmartDashboard.putData("Autonomous", m_chooser);
+    // Put subsystems to dashboard.
+    SmartDashboard.putData("Drivetrain", m_robotDrive);
+    SmartDashboard.putData("HatchSubsystem", m_hatchSubsystem);
+  }
+
+  /**
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   */
+  private void configureButtonBindings() {
+    // Grab the hatch when the 'A' button is pressed.
+    new JoystickButton(m_driverController, Button.kA.value).onTrue(new GrabHatch(m_hatchSubsystem));
+    // Release the hatch when the 'B' button is pressed.
+    new JoystickButton(m_driverController, Button.kB.value)
+        .onTrue(new ReleaseHatch(m_hatchSubsystem));
+    // While holding the shoulder button, drive at half speed
+    new JoystickButton(m_driverController, Button.kRightBumper.value)
+        .whileTrue(new HalveDriveSpeed(m_robotDrive));
+  }
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    return m_chooser.getSelected();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/commands/ComplexAuto.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
+
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+/** A complex auto command that drives forward, releases a hatch, and then drives backward. */
+public class ComplexAuto extends SequentialCommandGroup {
+  /**
+   * Creates a new ComplexAuto.
+   *
+   * @param drive The drive subsystem this command will run on
+   * @param hatch The hatch subsystem this command will run on
+   */
+  public ComplexAuto(DriveSubsystem drive, HatchSubsystem hatch) {
+    addCommands(
+        // Drive forward the specified distance
+        new DriveDistance(
+            AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed, drive),
+
+        // Release the hatch
+        new ReleaseHatch(hatch),
+
+        // Drive backward the specified distance
+        new DriveDistance(
+            AutoConstants.kAutoBackupDistanceInches, -AutoConstants.kAutoDriveSpeed, drive));
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/commands/DefaultDrive.java

@@ -0,0 +1,39 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
+
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+import java.util.function.DoubleSupplier;
+
+/**
+ * A command to drive the robot with joystick input (passed in as {@link DoubleSupplier}s). Written
+ * explicitly for pedagogical purposes - actual code should inline a command this simple with {@link
+ * edu.wpi.first.wpilibj2.command.RunCommand}.
+ */
+public class DefaultDrive extends Command {
+  private final DriveSubsystem m_drive;
+  private final DoubleSupplier m_forward;
+  private final DoubleSupplier m_rotation;
+
+  /**
+   * Creates a new DefaultDrive.
+   *
+   * @param subsystem The drive subsystem this command wil run on.
+   * @param forward The control input for driving forwards/backwards
+   * @param rotation The control input for turning
+   */
+  public DefaultDrive(DriveSubsystem subsystem, DoubleSupplier forward, DoubleSupplier rotation) {
+    m_drive = subsystem;
+    m_forward = forward;
+    m_rotation = rotation;
+    addRequirements(m_drive);
+  }
+
+  @Override
+  public void execute() {
+    m_drive.arcadeDrive(m_forward.getAsDouble(), m_rotation.getAsDouble());
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/commands/DriveDistance.java

@@ -0,0 +1,49 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
+
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class DriveDistance extends Command {
+  private final DriveSubsystem m_drive;
+  private final double m_distance;
+  private final double m_speed;
+
+  /**
+   * Creates a new DriveDistance.
+   *
+   * @param inches The number of inches the robot will drive
+   * @param speed The speed at which the robot will drive
+   * @param drive The drive subsystem on which this command will run
+   */
+  public DriveDistance(double inches, double speed, DriveSubsystem drive) {
+    m_distance = inches;
+    m_speed = speed;
+    m_drive = drive;
+    addRequirements(m_drive);
+  }
+
+  @Override
+  public void initialize() {
+    m_drive.resetEncoders();
+    m_drive.arcadeDrive(m_speed, 0);
+  }
+
+  @Override
+  public void execute() {
+    m_drive.arcadeDrive(m_speed, 0);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_drive.arcadeDrive(0, 0);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return Math.abs(m_drive.getAverageEncoderDistance()) >= m_distance;
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/commands/GrabHatch.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
+
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/**
+ * A simple command that grabs a hatch with the {@link HatchSubsystem}. Written explicitly for
+ * pedagogical purposes. Actual code should inline a command this simple with {@link
+ * edu.wpi.first.wpilibj2.command.InstantCommand}.
+ */
+public class GrabHatch extends Command {
+  // The subsystem the command runs on
+  private final HatchSubsystem m_hatchSubsystem;
+
+  public GrabHatch(HatchSubsystem subsystem) {
+    m_hatchSubsystem = subsystem;
+    addRequirements(m_hatchSubsystem);
+  }
+
+  @Override
+  public void initialize() {
+    m_hatchSubsystem.grabHatch();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/commands/HalveDriveSpeed.java

@@ -0,0 +1,26 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
+
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class HalveDriveSpeed extends Command {
+  private final DriveSubsystem m_drive;
+
+  public HalveDriveSpeed(DriveSubsystem drive) {
+    m_drive = drive;
+  }
+
+  @Override
+  public void initialize() {
+    m_drive.setMaxOutput(0.5);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_drive.setMaxOutput(1);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/commands/ReleaseHatch.java

@@ -0,0 +1,15 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
+
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+/** A command that releases the hatch. */
+public class ReleaseHatch extends InstantCommand {
+  public ReleaseHatch(HatchSubsystem subsystem) {
+    super(subsystem::releaseHatch, subsystem);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/subsystems/DriveSubsystem.java

@@ -0,0 +1,101 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class DriveSubsystem extends SubsystemBase {
+  // The motors on the left side of the drive.
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
+
+  // The motors on the right side of the drive.
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
+
+  // The robot's drive
+  private final DifferentialDrive m_drive =
+      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
+
+  // The left-side drive encoder
+  private final Encoder m_leftEncoder =
+      new Encoder(
+          DriveConstants.kLeftEncoderPorts[0],
+          DriveConstants.kLeftEncoderPorts[1],
+          DriveConstants.kLeftEncoderReversed);
+
+  // The right-side drive encoder
+  private final Encoder m_rightEncoder =
+      new Encoder(
+          DriveConstants.kRightEncoderPorts[0],
+          DriveConstants.kRightEncoderPorts[1],
+          DriveConstants.kRightEncoderReversed);
+
+  /** Creates a new DriveSubsystem. */
+  public DriveSubsystem() {
+    SendableRegistry.addChild(m_drive, m_leftLeader);
+    SendableRegistry.addChild(m_drive, m_rightLeader);
+
+    m_leftLeader.addFollower(m_leftFollower);
+    m_rightLeader.addFollower(m_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.
+    m_rightLeader.setInverted(true);
+
+    // Sets the distance per pulse for the encoders
+    m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+    m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+  }
+
+  /**
+   * Drives the robot using arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  public void arcadeDrive(double fwd, double rot) {
+    m_drive.arcadeDrive(fwd, rot);
+  }
+
+  /** Resets the drive encoders to currently read a position of 0. */
+  public void resetEncoders() {
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  /**
+   * Gets the average distance of the TWO encoders.
+   *
+   * @return the average of the TWO encoder readings
+   */
+  public double getAverageEncoderDistance() {
+    return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
+  }
+
+  /**
+   * 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
+   */
+  public void setMaxOutput(double maxOutput) {
+    m_drive.setMaxOutput(maxOutput);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+    // Publish encoder distances to telemetry.
+    builder.addDoubleProperty("leftDistance", m_leftEncoder::getDistance, null);
+    builder.addDoubleProperty("rightDistance", m_rightEncoder::getDistance, null);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/subsystems/HatchSubsystem.java

@@ -0,0 +1,41 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems;
+
+import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
+import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/** A hatch mechanism actuated by a single {@link DoubleSolenoid}. */
+public class HatchSubsystem extends SubsystemBase {
+  private final DoubleSolenoid m_hatchSolenoid =
+      new DoubleSolenoid(
+          0,
+          PneumaticsModuleType.CTREPCM,
+          HatchConstants.kHatchSolenoidPorts[0],
+          HatchConstants.kHatchSolenoidPorts[1]);
+
+  /** Grabs the hatch. */
+  public void grabHatch() {
+    m_hatchSolenoid.set(kForward);
+  }
+
+  /** Releases the hatch. */
+  public void releaseHatch() {
+    m_hatchSolenoid.set(kReverse);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+    // Publish the solenoid state to telemetry.
+    builder.addBooleanProperty("extended", () -> m_hatchSolenoid.get() == kForward, null);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hidrumble/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.hidrumble;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hidrumble/Robot.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.hidrumble;
+
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+/** This is a demo program showing the use of GenericHID's rumble feature. */
+public class Robot extends TimedRobot {
+  private final XboxController m_hid = new XboxController(0);
+
+  @Override
+  public void autonomousInit() {
+    // Turn on rumble at the start of auto
+    m_hid.setRumble(RumbleType.kLeftRumble, 1.0);
+    m_hid.setRumble(RumbleType.kRightRumble, 1.0);
+  }
+
+  @Override
+  public void disabledInit() {
+    // Stop the rumble when entering disabled
+    m_hid.setRumble(RumbleType.kLeftRumble, 0.0);
+    m_hid.setRumble(RumbleType.kRightRumble, 0.0);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/httpcamera/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.httpcamera;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/httpcamera/Robot.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.httpcamera;
+
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.cscore.CvSink;
+import edu.wpi.first.cscore.CvSource;
+import edu.wpi.first.cscore.HttpCamera;
+import edu.wpi.first.wpilibj.TimedRobot;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+
+/**
+ * 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.
+ */
+public class Robot extends TimedRobot {
+  Thread m_visionThread;
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    m_visionThread =
+        new Thread(
+            () -> {
+              // 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.
+              HttpCamera camera =
+                  new 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 cvSink = CameraServer.getVideo();
+              // Setup a CvSource. This will send images back to the Dashboard
+              CvSource outputStream = CameraServer.putVideo("Rectangle", 640, 480);
+
+              // Mats are very memory expensive. Lets reuse this Mat.
+              Mat mat = new Mat();
+
+              // This cannot be 'true'. The program will never exit if it is. This
+              // lets the robot stop this thread when restarting robot code or
+              // deploying.
+              while (!Thread.interrupted()) {
+                // 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
+                Imgproc.rectangle(
+                    mat, new Point(100, 100), new Point(400, 400), new Scalar(255, 255, 255), 5);
+                // Give the output stream a new image to display
+                outputStream.putFrame(mat);
+              }
+            });
+    m_visionThread.setDaemon(true);
+    m_visionThread.start();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/i2ccommunication/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.i2ccommunication;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/i2ccommunication/Robot.java

@@ -0,0 +1,74 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.i2ccommunication;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.I2C.Port;
+import edu.wpi.first.wpilibj.TimedRobot;
+import java.util.Optional;
+
+/**
+ * This is a sample program demonstrating how to communicate to a light controller from the robot
+ * code using the roboRIO's I2C port.
+ */
+public class Robot extends TimedRobot {
+  static final Port kPort = Port.kPort0;
+  private static final int kDeviceAddress = 4;
+
+  private final I2C m_arduino = new I2C(kPort, kDeviceAddress);
+
+  private void writeString(String input) {
+    // Creates a char array from the input string
+    char[] chars = input.toCharArray();
+
+    // Creates a byte array from the char array
+    byte[] data = new byte[chars.length];
+
+    // Adds each character
+    for (int i = 0; i < chars.length; i++) {
+      data[i] = (byte) chars[i];
+    }
+
+    // Writes bytes over I2C
+    m_arduino.transaction(data, data.length, new byte[] {}, 0);
+  }
+
+  @Override
+  @SuppressWarnings("PMD.ConsecutiveLiteralAppends")
+  public void robotPeriodic() {
+    // 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.
+    StringBuilder stateMessage = new StringBuilder(6);
+
+    String allianceString = "U";
+    Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
+    if (alliance.isPresent()) {
+      allianceString = alliance.get() == DriverStation.Alliance.Red ? "R" : "B";
+    }
+
+    stateMessage
+        .append(allianceString)
+        .append(DriverStation.isEnabled() ? "E" : "D")
+        .append(DriverStation.isAutonomous() ? "A" : "T")
+        .append(String.format("%03d", (int) DriverStation.getMatchTime()));
+
+    writeString(stateMessage.toString());
+  }
+
+  /** Close all resources. */
+  @Override
+  public void close() {
+    m_arduino.close();
+    super.close();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/intermediatevision/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.intermediatevision;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/intermediatevision/Robot.java

@@ -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.
+
+package edu.wpi.first.wpilibj.examples.intermediatevision;
+
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.cscore.CvSink;
+import edu.wpi.first.cscore.CvSource;
+import edu.wpi.first.cscore.UsbCamera;
+import edu.wpi.first.wpilibj.TimedRobot;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+
+/**
+ * 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.
+ */
+public class Robot extends TimedRobot {
+  Thread m_visionThread;
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    m_visionThread =
+        new Thread(
+            () -> {
+              // Get the UsbCamera from CameraServer
+              UsbCamera camera = CameraServer.startAutomaticCapture();
+              // Set the resolution
+              camera.setResolution(640, 480);
+
+              // Get a CvSink. This will capture Mats from the camera
+              CvSink cvSink = CameraServer.getVideo();
+              // Setup a CvSource. This will send images back to the Dashboard
+              CvSource outputStream = CameraServer.putVideo("Rectangle", 640, 480);
+
+              // Mats are very memory expensive. Lets reuse this Mat.
+              Mat mat = new Mat();
+
+              // This cannot be 'true'. The program will never exit if it is. This
+              // lets the robot stop this thread when restarting robot code or
+              // deploying.
+              while (!Thread.interrupted()) {
+                // 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
+                Imgproc.rectangle(
+                    mat, new Point(100, 100), new Point(400, 400), new Scalar(255, 255, 255), 5);
+                // Give the output stream a new image to display
+                outputStream.putFrame(mat);
+              }
+            });
+    m_visionThread.setDaemon(true);
+    m_visionThread.start();
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/mecanumbot/Drivetrain.java

@@ -0,0 +1,139 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.mecanumbot;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.OnboardIMU;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+
+/** Represents a mecanum drive style drivetrain. */
+public class Drivetrain {
+  public static final double kMaxSpeed = 3.0; // 3 meters per second
+  public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
+
+  private final PWMSparkMax m_frontLeftMotor = new PWMSparkMax(1);
+  private final PWMSparkMax m_frontRightMotor = new PWMSparkMax(2);
+  private final PWMSparkMax m_backLeftMotor = new PWMSparkMax(3);
+  private final PWMSparkMax m_backRightMotor = new PWMSparkMax(4);
+
+  private final Encoder m_frontLeftEncoder = new Encoder(0, 1);
+  private final Encoder m_frontRightEncoder = new Encoder(2, 3);
+  private final Encoder m_backLeftEncoder = new Encoder(4, 5);
+  private final Encoder m_backRightEncoder = new Encoder(6, 7);
+
+  private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
+  private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
+  private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
+  private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
+
+  private final PIDController m_frontLeftPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_frontRightPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_backLeftPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_backRightPIDController = new PIDController(1, 0, 0);
+
+  private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.kFlat);
+
+  private final MecanumDriveKinematics m_kinematics =
+      new MecanumDriveKinematics(
+          m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
+
+  private final MecanumDriveOdometry m_odometry =
+      new MecanumDriveOdometry(m_kinematics, m_imu.getRotation2d(), getCurrentDistances());
+
+  // Gains are for example purposes only - must be determined for your own robot!
+  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
+
+  /** Constructs a MecanumDrive and resets the gyro. */
+  public Drivetrain() {
+    m_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.
+    m_frontRightMotor.setInverted(true);
+    m_backRightMotor.setInverted(true);
+  }
+
+  /**
+   * Returns the current state of the drivetrain.
+   *
+   * @return The current state of the drivetrain.
+   */
+  public MecanumDriveWheelSpeeds getCurrentState() {
+    return new MecanumDriveWheelSpeeds(
+        m_frontLeftEncoder.getRate(),
+        m_frontRightEncoder.getRate(),
+        m_backLeftEncoder.getRate(),
+        m_backRightEncoder.getRate());
+  }
+
+  /**
+   * Returns the current distances measured by the drivetrain.
+   *
+   * @return The current distances measured by the drivetrain.
+   */
+  public MecanumDriveWheelPositions getCurrentDistances() {
+    return new MecanumDriveWheelPositions(
+        m_frontLeftEncoder.getDistance(),
+        m_frontRightEncoder.getDistance(),
+        m_backLeftEncoder.getDistance(),
+        m_backRightEncoder.getDistance());
+  }
+
+  /**
+   * Set the desired speeds for each wheel.
+   *
+   * @param speeds The desired wheel speeds.
+   */
+  public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
+    final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeft);
+    final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRight);
+    final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeft);
+    final double backRightFeedforward = m_feedforward.calculate(speeds.rearRight);
+
+    final double frontLeftOutput =
+        m_frontLeftPIDController.calculate(m_frontLeftEncoder.getRate(), speeds.frontLeft);
+    final double frontRightOutput =
+        m_frontRightPIDController.calculate(m_frontRightEncoder.getRate(), speeds.frontRight);
+    final double backLeftOutput =
+        m_backLeftPIDController.calculate(m_backLeftEncoder.getRate(), speeds.rearLeft);
+    final double backRightOutput =
+        m_backRightPIDController.calculate(m_backRightEncoder.getRate(), speeds.rearRight);
+
+    m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward);
+    m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward);
+    m_backLeftMotor.setVoltage(backLeftOutput + backLeftFeedforward);
+    m_backRightMotor.setVoltage(backRightOutput + backRightFeedforward);
+  }
+
+  /**
+   * 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.
+   */
+  public void drive(
+      double xSpeed, double ySpeed, double rot, boolean fieldRelative, double period) {
+    var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
+    if (fieldRelative) {
+      chassisSpeeds = chassisSpeeds.toRobotRelative(m_imu.getRotation2d());
+    }
+    setSpeeds(m_kinematics.toWheelSpeeds(chassisSpeeds.discretize(period)).desaturate(kMaxSpeed));
+  }
+
+  /** Updates the field relative position of the robot. */
+  public void updateOdometry() {
+    m_odometry.update(m_imu.getRotation2d(), getCurrentDistances());
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/mecanumbot/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.mecanumbot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}