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 org.wpilib.framework;
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
067 * org.wpilib.system.Threads#setCurrentThreadPriority(boolean,int)}. An RT priority of 15 is a
068 * reasonable choice.
069 *
070 * <p>If you do enable RT though, <i>make sure your periodic functions do not block</i>. If they do,
071 * the operating system will lock up, and you'll have to boot the roboRIO into safe mode and delete
072 * the robot program to recover.
073 */
074public class TimesliceRobot extends TimedRobot {
075  /**
076   * Constructor for TimesliceRobot.
077   *
078   * @param robotPeriodicAllocation The allocation in seconds to give the TimesliceRobot periodic
079   *     functions.
080   * @param controllerPeriod The controller period in seconds. The sum of all scheduler allocations
081   *     should be less than or equal to this value.
082   */
083  public TimesliceRobot(double robotPeriodicAllocation, double controllerPeriod) {
084    m_nextOffset = robotPeriodicAllocation;
085    m_controllerPeriod = controllerPeriod;
086  }
087
088  /**
089   * Schedule a periodic function with the constructor's controller period and the given allocation.
090   * The function's runtime allocation will be placed after the end of the previous one's.
091   *
092   * <p>If a call to this function makes the allocations exceed the controller period, an exception
093   * will be thrown since that means the TimesliceRobot periodic functions and the given function
094   * will have conflicting timeslices.
095   *
096   * @param func Function to schedule.
097   * @param allocation The function's runtime allocation in seconds out of the controller period.
098   */
099  public void schedule(Runnable func, double allocation) {
100    if (m_nextOffset + allocation > m_controllerPeriod) {
101      throw new IllegalStateException(
102          "Function scheduled at offset "
103              + m_nextOffset
104              + " with allocation "
105              + allocation
106              + " exceeded controller period of "
107              + m_controllerPeriod
108              + "\n");
109    }
110
111    addPeriodic(func, m_controllerPeriod, m_nextOffset);
112    m_nextOffset += allocation;
113  }
114
115  private double m_nextOffset;
116  private final double m_controllerPeriod;
117}