[wpilib] Add TimesliceRobot (#3502)

wpilibj/src/main/java/edu/wpi/first/wpilibj/TimesliceRobot.java

@@ -0,0 +1,116 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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;
+
+/**
+ * TimesliceRobot extends the TimedRobot robot program framework to provide timeslice scheduling of
+ * periodic functions.
+ *
+ * <p>The TimesliceRobot class is intended to be subclassed by a user creating a robot program.
+ *
+ * <p>This class schedules robot operations serially in a timeslice format. TimedRobot's periodic
+ * functions are the first in the timeslice table with 0 ms offset and 20 ms period. You can
+ * schedule additional controller periodic functions at a shorter period (5 ms by default). You give
+ * each one a timeslice duration, then they're run sequentially. The main benefit of this approach
+ * is consistent starting times for each controller periodic, which can make odometry and estimators
+ * more accurate and controller outputs change more consistently.
+ *
+ * <p>Here's an example of measured subsystem durations and their timeslice allocations:
+ *
+ * <table>
+ *   <tr>
+ *     <td><b>Subsystem</b></td>
+ *     <td><b>Duration (ms)</b></td>
+ *     <td><b>Allocation (ms)</b></td>
+ *   </tr>
+ *   <tr>
+ *     <td><b>Total</b></td>
+ *     <td>5.0</td>
+ *     <td>5.0</td>
+ *   </tr>
+ *   <tr>
+ *     <td>TimedRobot</td>
+ *     <td>?</td>
+ *     <td>2.0</td>
+ *   </tr>
+ *   <tr>
+ *     <td>Drivetrain</td>
+ *     <td>1.32</td>
+ *     <td>1.5</td>
+ *   </tr>
+ *   <tr>
+ *     <td>Flywheel</td>
+ *     <td>0.6</td>
+ *     <td>0.7</td>
+ *   </tr>
+ *   <tr>
+ *     <td>Turret</td>
+ *     <td>0.6</td>
+ *     <td>0.8</td>
+ *   </tr>
+ *   <tr>
+ *     <td><b>Free</b></td>
+ *     <td>0.0</td>
+ *     <td>N/A</td>
+ *   </tr>
+ * </table>
+ *
+ * <p>Since TimedRobot periodic functions only run every 20ms, that leaves a 2 ms empty spot in the
+ * allocation table for three of the four 5 ms cycles comprising 20 ms. That's OK because the OS
+ * needs time to do other things.
+ *
+ * <p>If the robot periodic functions and the controller periodic functions have a lot of scheduling
+ * jitter that cause them to occasionally overlap with later timeslices, consider giving the main
+ * robot thread a real-time priority using {@link Threads#setCurrentThreadPriority(boolean,int)}. An
+ * RT priority of 15 is a reasonable choice.
+ *
+ * <p>If you do enable RT though, <i>make sure your periodic functions do not block</i>. If they do,
+ * the operating system will lock up, and you'll have to boot the roboRIO into safe mode and delete
+ * the robot program to recover.
+ */
+public class TimesliceRobot extends TimedRobot {
+  /**
+   * Constructor for TimesliceRobot.
+   *
+   * @param robotPeriodicAllocation The allocation in seconds to give the TimesliceRobot periodic
+   *     functions.
+   * @param controllerPeriod The controller period in seconds. The sum of all scheduler allocations
+   *     should be less than or equal to this value.
+   */
+  public TimesliceRobot(double robotPeriodicAllocation, double controllerPeriod) {
+    m_nextOffset = robotPeriodicAllocation;
+    m_controllerPeriod = controllerPeriod;
+  }
+
+  /**
+   * Schedule a periodic function with the constructor's controller period and the given allocation.
+   * The function's runtime allocation will be placed after the end of the previous one's.
+   *
+   * <p>If a call to this function makes the allocations exceed the controller period, an exception
+   * will be thrown since that means the TimesliceRobot periodic functions and the given function
+   * will have conflicting timeslices.
+   *
+   * @param func Function to schedule.
+   * @param allocation The function's runtime allocation in seconds out of the controller period.
+   */
+  public void schedule(Runnable func, double allocation) {
+    if (m_nextOffset + allocation > m_controllerPeriod) {
+      throw new IllegalStateException(
+          "Function scheduled at offset "
+              + m_nextOffset
+              + " with allocation "
+              + allocation
+              + " exceeded controller period of "
+              + m_controllerPeriod
+              + "\n");
+    }
+
+    addPeriodic(func, m_controllerPeriod, m_nextOffset);
+    m_nextOffset += allocation;
+  }
+
+  private double m_nextOffset;
+  private final double m_controllerPeriod;
+}