diff --git a/wpilibc/src/main/native/cpp/TimesliceRobot.cpp b/wpilibc/src/main/native/cpp/TimesliceRobot.cpp new file mode 100644 index 0000000000..d212c1015f --- /dev/null +++ b/wpilibc/src/main/native/cpp/TimesliceRobot.cpp @@ -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. + +#include "frc/TimesliceRobot.h" + +#include "frc/Errors.h" +#include "frc/fmt/Units.h" + +using namespace frc; + +TimesliceRobot::TimesliceRobot(units::second_t robotPeriodicAllocation, + units::second_t controllerPeriod) + : m_nextOffset{robotPeriodicAllocation}, + m_controllerPeriod{controllerPeriod} {} + +void TimesliceRobot::Schedule(std::function func, + units::second_t allocation) { + if (m_nextOffset + allocation > m_controllerPeriod) { + throw FRC_MakeError(err::Error, + "Function scheduled at offset {} with allocation {} " + "exceeded controller period of {}\n", + m_nextOffset, allocation, m_controllerPeriod); + } + + AddPeriodic(func, m_controllerPeriod, m_nextOffset); + m_nextOffset += allocation; +} diff --git a/wpilibc/src/main/native/include/frc/TimesliceRobot.h b/wpilibc/src/main/native/include/frc/TimesliceRobot.h new file mode 100644 index 0000000000..bc045d58b6 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/TimesliceRobot.h @@ -0,0 +1,120 @@ +// 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. + +#pragma once + +#include + +#include + +#include "frc/TimedRobot.h" + +namespace frc { + +/** + * TimesliceRobot extends the TimedRobot robot program framework to provide + * timeslice scheduling of periodic functions. + * + * The TimesliceRobot class is intended to be subclassed by a user creating a + * robot program. + * + * 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. + * + * Here's an example of measured subsystem durations and their timeslice + * allocations: + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
SubsystemDuration (ms)Allocation (ms)
Total5.05.0
TimedRobot?2.0
Drivetrain1.321.5
Flywheel0.60.7
Turret0.60.8
Free0.0N/A
+ * + * 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. + * + * 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 + * frc::SetCurrentThreadPriority(). An RT priority of 15 is a reasonable choice. + * + * If you do enable RT though, make sure your periodic functions do not + * block. 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. + */ +class TimesliceRobot : public TimedRobot { + public: + /** + * Constructor for TimesliceRobot. + * + * @param robotPeriodicAllocation The allocation to give the TimesliceRobot + * periodic functions. + * @param controllerPeriod The controller period. The sum of all scheduler + * allocations should be less than or equal to this + * value. + */ + explicit TimesliceRobot(units::second_t robotPeriodicAllocation, + units::second_t 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. + * + * 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 out of the controller + * period. + */ + void Schedule(std::function func, units::second_t allocation); + + private: + units::second_t m_nextOffset; + units::second_t m_controllerPeriod; +}; + +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/TimesliceRobotTest.cpp b/wpilibc/src/test/native/cpp/TimesliceRobotTest.cpp new file mode 100644 index 0000000000..a2e4e13eda --- /dev/null +++ b/wpilibc/src/test/native/cpp/TimesliceRobotTest.cpp @@ -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. + +#include "frc/TimesliceRobot.h" // NOLINT(build/include_order) + +#include + +#include +#include + +#include "frc/simulation/DriverStationSim.h" +#include "frc/simulation/SimHooks.h" +#include "gtest/gtest.h" + +using namespace frc; + +namespace { +class TimesliceRobotTest : public ::testing::Test { + protected: + void SetUp() override { frc::sim::PauseTiming(); } + + void TearDown() override { frc::sim::ResumeTiming(); } +}; + +class MockRobot : public TimesliceRobot { + public: + std::atomic m_robotPeriodicCount{0}; + + MockRobot() : TimesliceRobot{2_ms, 5_ms} {} + + void RobotPeriodic() override { m_robotPeriodicCount++; } +}; +} // namespace + +TEST_F(TimesliceRobotTest, Schedule) { + MockRobot robot; + + std::atomic callbackCount1{0}; + std::atomic callbackCount2{0}; + + // Timeslice allocation table + // + // | Name | Offset (ms) | Allocation (ms)| + // |-----------------|-------------|----------------| + // | RobotPeriodic() | 0 | 2 | + // | Callback 1 | 2 | 0.5 | + // | Callback 2 | 2.5 | 1 | + robot.Schedule([&] { callbackCount1++; }, 0.5_ms); + robot.Schedule([&] { callbackCount2++; }, 1_ms); + + std::thread robotThread{[&] { robot.StartCompetition(); }}; + + frc::sim::DriverStationSim::SetEnabled(false); + frc::sim::DriverStationSim::NotifyNewData(); + frc::sim::StepTiming(0_ms); // Wait for Notifiers + + // Functions scheduled with addPeriodic() are delayed by one period before + // their first run (5 ms for this test's callbacks here and 20 ms for + // robotPeriodic()). + frc::sim::StepTiming(5_ms); + + EXPECT_EQ(0u, robot.m_robotPeriodicCount); + EXPECT_EQ(0u, callbackCount1); + EXPECT_EQ(0u, callbackCount2); + + // Step to 1.5 ms + frc::sim::StepTiming(1.5_ms); + EXPECT_EQ(0u, robot.m_robotPeriodicCount); + EXPECT_EQ(0u, callbackCount1); + EXPECT_EQ(0u, callbackCount2); + + // Step to 2.25 ms + frc::sim::StepTiming(0.75_ms); + EXPECT_EQ(0u, robot.m_robotPeriodicCount); + EXPECT_EQ(1u, callbackCount1); + EXPECT_EQ(0u, callbackCount2); + + // Step to 2.75 ms + frc::sim::StepTiming(0.5_ms); + EXPECT_EQ(0u, robot.m_robotPeriodicCount); + EXPECT_EQ(1u, callbackCount1); + EXPECT_EQ(1u, callbackCount2); + + robot.EndCompetition(); + robotThread.join(); +} + +TEST_F(TimesliceRobotTest, ScheduleOverrun) { + MockRobot robot; + + robot.Schedule([] {}, 0.5_ms); + robot.Schedule([] {}, 1_ms); + + // offset = 2 ms + 0.5 ms + 1 ms = 3.5 ms + // 3.5 ms + 3 ms allocation = 6.5 ms > max of 5 ms + EXPECT_THROW(robot.Schedule([] {}, 3_ms), std::runtime_error); + + robot.EndCompetition(); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimesliceRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimesliceRobot.java new file mode 100644 index 0000000000..5d10bb57eb --- /dev/null +++ b/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. + * + *

The TimesliceRobot class is intended to be subclassed by a user creating a robot program. + * + *

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. + * + *

Here's an example of measured subsystem durations and their timeslice allocations: + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
SubsystemDuration (ms)Allocation (ms)
Total5.05.0
TimedRobot?2.0
Drivetrain1.321.5
Flywheel0.60.7
Turret0.60.8
Free0.0N/A
+ * + *

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. + * + *

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. + * + *

If you do enable RT though, make sure your periodic functions do not block. 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. + * + *

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; +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimesliceRobotTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimesliceRobotTest.java new file mode 100644 index 0000000000..ee4860deb7 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimesliceRobotTest.java @@ -0,0 +1,131 @@ +// 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; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; + +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj.simulation.SimHooks; +import java.util.concurrent.atomic.AtomicInteger; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.parallel.ResourceLock; + +class TimesliceRobotTest { + static class MockRobot extends TimesliceRobot { + public final AtomicInteger m_robotPeriodicCount = new AtomicInteger(0); + + MockRobot() { + super(0.002, 0.005); + } + + @Override + public void robotPeriodic() { + m_robotPeriodicCount.addAndGet(1); + } + } + + @BeforeEach + void setup() { + SimHooks.pauseTiming(); + } + + @AfterEach + void cleanup() { + SimHooks.resumeTiming(); + } + + @Test + @ResourceLock("timing") + void scheduleTest() { + MockRobot robot = new MockRobot(); + + final AtomicInteger callbackCount1 = new AtomicInteger(0); + final AtomicInteger callbackCount2 = new AtomicInteger(0); + + // Timeslice allocation table + // + // | Name | Offset (ms) | Allocation (ms)| + // |-----------------|-------------|----------------| + // | RobotPeriodic() | 0 | 2 | + // | Callback 1 | 2 | 0.5 | + // | Callback 2 | 2.5 | 1 | + robot.schedule( + () -> { + callbackCount1.addAndGet(1); + }, + 0.0005); + robot.schedule( + () -> { + callbackCount2.addAndGet(1); + }, + 0.001); + + Thread robotThread = + new Thread( + () -> { + robot.startCompetition(); + }); + robotThread.start(); + + DriverStationSim.setEnabled(false); + DriverStationSim.notifyNewData(); + SimHooks.stepTiming(0.0); // Wait for Notifiers + + // Functions scheduled with addPeriodic() are delayed by one period before + // their first run (5 ms for this test's callbacks here and 20 ms for + // robotPeriodic()). + SimHooks.stepTiming(0.005); + + assertEquals(0, robot.m_robotPeriodicCount.get()); + assertEquals(0, callbackCount1.get()); + assertEquals(0, callbackCount2.get()); + + // Step to 1.5 ms + SimHooks.stepTiming(0.0015); + assertEquals(0, robot.m_robotPeriodicCount.get()); + assertEquals(0, callbackCount1.get()); + assertEquals(0, callbackCount2.get()); + + // Step to 2.25 ms + SimHooks.stepTiming(0.00075); + assertEquals(0, robot.m_robotPeriodicCount.get()); + assertEquals(1, callbackCount1.get()); + assertEquals(0, callbackCount2.get()); + + // Step to 2.75 ms + SimHooks.stepTiming(0.0005); + assertEquals(0, robot.m_robotPeriodicCount.get()); + assertEquals(1, callbackCount1.get()); + assertEquals(1, callbackCount2.get()); + + robot.endCompetition(); + try { + robotThread.interrupt(); + robotThread.join(); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + robot.close(); + } + + @Test + @ResourceLock("timing") + void scheduleOverrunTest() { + MockRobot robot = new MockRobot(); + + robot.schedule(() -> {}, 0.0005); + robot.schedule(() -> {}, 0.001); + + // offset = 2 ms + 0.5 ms + 1 ms = 3.5 ms + // 3.5 ms + 3 ms allocation = 6.5 ms > max of 5 ms + assertThrows(IllegalStateException.class, () -> robot.schedule(() -> {}, 0.003)); + + robot.endCompetition(); + robot.close(); + } +}