Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add TimesliceRobot (#3502)

Browse Source
This commit is contained in:
Tyler Veness
2021-09-26 13:56:33 -07:00
committed by GitHub
parent af295879fb
commit 173cb7359d
5 changed files with 495 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
wpilibc/src/main/native/cpp/TimesliceRobot.cpp Normal file
View File

@@ -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<void()> 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;
}

120
wpilibc/src/main/native/include/frc/TimesliceRobot.h Normal file
View File

@@ -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 <functional>
#include <units/time.h>
#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:
*
* <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>
*
* 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, <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.
*/
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<void()> func, units::second_t allocation);
private:
units::second_t m_nextOffset;
units::second_t m_controllerPeriod;
};
} // namespace frc

100
wpilibc/src/test/native/cpp/TimesliceRobotTest.cpp Normal file
View File

@@ -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 <stdint.h>
#include <atomic>
#include <thread>
#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<uint32_t> m_robotPeriodicCount{0};
MockRobot() : TimesliceRobot{2_ms, 5_ms} {}
void RobotPeriodic() override { m_robotPeriodicCount++; }
};
} // namespace
TEST_F(TimesliceRobotTest, Schedule) {
MockRobot robot;
std::atomic<uint32_t> callbackCount1{0};
std::atomic<uint32_t> 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();
}

116
wpilibj/src/main/java/edu/wpi/first/wpilibj/TimesliceRobot.java Normal file
View File

@@ -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;
}

131
wpilibj/src/test/java/edu/wpi/first/wpilibj/TimesliceRobotTest.java Normal file
View File

@@ -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();
}
}