| Austin Schuh | 812d0d1 | 2021-11-04 20:16:48 -0700 | [diff] [blame] | 1 | // Copyright (c) FIRST and other WPILib contributors. |
| 2 | // Open Source Software; you can modify and/or share it under the terms of |
| 3 | // the WPILib BSD license file in the root directory of this project. |
| 4 | |
| 5 | #include "frc/TimesliceRobot.h" |
| 6 | |
| 7 | #include "frc/Errors.h" |
| 8 | #include "frc/fmt/Units.h" |
| 9 | |
| 10 | using namespace frc; |
| 11 | |
| 12 | TimesliceRobot::TimesliceRobot(units::second_t robotPeriodicAllocation, |
| 13 | units::second_t controllerPeriod) |
| 14 | : m_nextOffset{robotPeriodicAllocation}, |
| 15 | m_controllerPeriod{controllerPeriod} {} |
| 16 | |
| 17 | void TimesliceRobot::Schedule(std::function<void()> func, |
| 18 | units::second_t allocation) { |
| 19 | if (m_nextOffset + allocation > m_controllerPeriod) { |
| 20 | throw FRC_MakeError(err::Error, |
| 21 | "Function scheduled at offset {} with allocation {} " |
| 22 | "exceeded controller period of {}\n", |
| 23 | m_nextOffset, allocation, m_controllerPeriod); |
| 24 | } |
| 25 | |
| 26 | AddPeriodic(func, m_controllerPeriod, m_nextOffset); |
| 27 | m_nextOffset += allocation; |
| 28 | } |