001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj;
006
007/**
008 * TimesliceRobot extends the TimedRobot robot program framework to provide timeslice scheduling of
009 * periodic functions.
010 *
011 * <p>The TimesliceRobot class is intended to be subclassed by a user creating a robot program.
012 *
013 * <p>This class schedules robot operations serially in a timeslice format. TimedRobot's periodic
014 * functions are the first in the timeslice table with 0 ms offset and 20 ms period. You can
015 * schedule additional controller periodic functions at a shorter period (5 ms by default). You give
016 * each one a timeslice duration, then they're run sequentially. The main benefit of this approach
017 * is consistent starting times for each controller periodic, which can make odometry and estimators
018 * more accurate and controller outputs change more consistently.
019 *
020 * <p>Here's an example of measured subsystem durations and their timeslice allocations:
021 *
022 * <table>
023 *   <tr>
024 *     <td><b>Subsystem</b></td>
025 *     <td><b>Duration (ms)</b></td>
026 *     <td><b>Allocation (ms)</b></td>
027 *   </tr>
028 *   <tr>
029 *     <td><b>Total</b></td>
030 *     <td>5.0</td>
031 *     <td>5.0</td>
032 *   </tr>
033 *   <tr>
034 *     <td>TimedRobot</td>
035 *     <td>?</td>
036 *     <td>2.0</td>
037 *   </tr>
038 *   <tr>
039 *     <td>Drivetrain</td>
040 *     <td>1.32</td>
041 *     <td>1.5</td>
042 *   </tr>
043 *   <tr>
044 *     <td>Flywheel</td>
045 *     <td>0.6</td>
046 *     <td>0.7</td>
047 *   </tr>
048 *   <tr>
049 *     <td>Turret</td>
050 *     <td>0.6</td>
051 *     <td>0.8</td>
052 *   </tr>
053 *   <tr>
054 *     <td><b>Free</b></td>
055 *     <td>0.0</td>
056 *     <td>N/A</td>
057 *   </tr>
058 * </table>
059 *
060 * <p>Since TimedRobot periodic functions only run every 20ms, that leaves a 2 ms empty spot in the
061 * allocation table for three of the four 5 ms cycles comprising 20 ms. That's OK because the OS
062 * needs time to do other things.
063 *
064 * <p>If the robot periodic functions and the controller periodic functions have a lot of scheduling
065 * jitter that cause them to occasionally overlap with later timeslices, consider giving the main
066 * robot thread a real-time priority using {@link Threads#setCurrentThreadPriority(boolean,int)}. An
067 * RT priority of 15 is a reasonable choice.
068 *
069 * <p>If you do enable RT though, <i>make sure your periodic functions do not block</i>. If they do,
070 * the operating system will lock up, and you'll have to boot the roboRIO into safe mode and delete
071 * the robot program to recover.
072 */
073public class TimesliceRobot extends TimedRobot {
074  /**
075   * Constructor for TimesliceRobot.
076   *
077   * @param robotPeriodicAllocation The allocation in seconds to give the TimesliceRobot periodic
078   *     functions.
079   * @param controllerPeriod The controller period in seconds. The sum of all scheduler allocations
080   *     should be less than or equal to this value.
081   */
082  public TimesliceRobot(double robotPeriodicAllocation, double controllerPeriod) {
083    m_nextOffset = robotPeriodicAllocation;
084    m_controllerPeriod = controllerPeriod;
085  }
086
087  /**
088   * Schedule a periodic function with the constructor's controller period and the given allocation.
089   * The function's runtime allocation will be placed after the end of the previous one's.
090   *
091   * <p>If a call to this function makes the allocations exceed the controller period, an exception
092   * will be thrown since that means the TimesliceRobot periodic functions and the given function
093   * will have conflicting timeslices.
094   *
095   * @param func Function to schedule.
096   * @param allocation The function's runtime allocation in seconds out of the controller period.
097   */
098  public void schedule(Runnable func, double allocation) {
099    if (m_nextOffset + allocation > m_controllerPeriod) {
100      throw new IllegalStateException(
101          "Function scheduled at offset "
102              + m_nextOffset
103              + " with allocation "
104              + allocation
105              + " exceeded controller period of "
106              + m_controllerPeriod
107              + "\n");
108    }
109
110    addPeriodic(func, m_controllerPeriod, m_nextOffset);
111    m_nextOffset += allocation;
112  }
113
114  private double m_nextOffset;
115  private final double m_controllerPeriod;
116}