[wpilibc] Move framework sources to framework directory (#8347)

wpilibc/src/main/native/cpp/framework/IterativeRobotBase.cpp

@@ -0,0 +1,202 @@
+// 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 "wpi/framework/IterativeRobotBase.hpp"
+
+#include "wpi/driverstation/DSControlWord.hpp"
+#include "wpi/driverstation/DriverStation.hpp"
+#include "wpi/hal/DriverStation.h"
+#include "wpi/nt/NetworkTableInstance.hpp"
+#include "wpi/smartdashboard/SmartDashboard.hpp"
+#include "wpi/system/Errors.hpp"
+#include "wpi/util/print.hpp"
+
+using namespace wpi;
+
+IterativeRobotBase::IterativeRobotBase(wpi::units::second_t period)
+    : m_period(period),
+      m_watchdog(period, [this] { PrintLoopOverrunMessage(); }) {}
+
+void IterativeRobotBase::DriverStationConnected() {}
+
+void IterativeRobotBase::SimulationInit() {}
+
+void IterativeRobotBase::DisabledInit() {}
+
+void IterativeRobotBase::AutonomousInit() {}
+
+void IterativeRobotBase::TeleopInit() {}
+
+void IterativeRobotBase::TestInit() {}
+
+void IterativeRobotBase::RobotPeriodic() {
+  static bool firstRun = true;
+  if (firstRun) {
+    wpi::util::print("Default {}() method... Override me!\n", __FUNCTION__);
+    firstRun = false;
+  }
+}
+
+void IterativeRobotBase::SimulationPeriodic() {
+  static bool firstRun = true;
+  if (firstRun) {
+    wpi::util::print("Default {}() method... Override me!\n", __FUNCTION__);
+    firstRun = false;
+  }
+}
+
+void IterativeRobotBase::DisabledPeriodic() {
+  static bool firstRun = true;
+  if (firstRun) {
+    wpi::util::print("Default {}() method... Override me!\n", __FUNCTION__);
+    firstRun = false;
+  }
+}
+
+void IterativeRobotBase::AutonomousPeriodic() {
+  static bool firstRun = true;
+  if (firstRun) {
+    wpi::util::print("Default {}() method... Override me!\n", __FUNCTION__);
+    firstRun = false;
+  }
+}
+
+void IterativeRobotBase::TeleopPeriodic() {
+  static bool firstRun = true;
+  if (firstRun) {
+    wpi::util::print("Default {}() method... Override me!\n", __FUNCTION__);
+    firstRun = false;
+  }
+}
+
+void IterativeRobotBase::TestPeriodic() {
+  static bool firstRun = true;
+  if (firstRun) {
+    wpi::util::print("Default {}() method... Override me!\n", __FUNCTION__);
+    firstRun = false;
+  }
+}
+
+void IterativeRobotBase::DisabledExit() {}
+
+void IterativeRobotBase::AutonomousExit() {}
+
+void IterativeRobotBase::TeleopExit() {}
+
+void IterativeRobotBase::TestExit() {}
+
+void IterativeRobotBase::SetNetworkTablesFlushEnabled(bool enabled) {
+  m_ntFlushEnabled = enabled;
+}
+
+wpi::units::second_t IterativeRobotBase::GetPeriod() const {
+  return m_period;
+}
+
+void IterativeRobotBase::LoopFunc() {
+  DriverStation::RefreshData();
+  m_watchdog.Reset();
+
+  // Get current mode
+  DSControlWord word;
+  Mode mode = Mode::kNone;
+  if (word.IsDisabled()) {
+    mode = Mode::kDisabled;
+  } else if (word.IsAutonomous()) {
+    mode = Mode::kAutonomous;
+  } else if (word.IsTeleop()) {
+    mode = Mode::kTeleop;
+  } else if (word.IsTest()) {
+    mode = Mode::kTest;
+  }
+
+  if (!m_calledDsConnected && word.IsDSAttached()) {
+    m_calledDsConnected = true;
+    DriverStationConnected();
+  }
+
+  // If mode changed, call mode exit and entry functions
+  if (m_lastMode != mode) {
+    // Call last mode's exit function
+    if (m_lastMode == Mode::kDisabled) {
+      DisabledExit();
+    } else if (m_lastMode == Mode::kAutonomous) {
+      AutonomousExit();
+    } else if (m_lastMode == Mode::kTeleop) {
+      TeleopExit();
+    } else if (m_lastMode == Mode::kTest) {
+      TestExit();
+    }
+
+    // Call current mode's entry function
+    if (mode == Mode::kDisabled) {
+      DisabledInit();
+      m_watchdog.AddEpoch("DisabledInit()");
+    } else if (mode == Mode::kAutonomous) {
+      AutonomousInit();
+      m_watchdog.AddEpoch("AutonomousInit()");
+    } else if (mode == Mode::kTeleop) {
+      TeleopInit();
+      m_watchdog.AddEpoch("TeleopInit()");
+    } else if (mode == Mode::kTest) {
+      TestInit();
+      m_watchdog.AddEpoch("TestInit()");
+    }
+
+    m_lastMode = mode;
+  }
+
+  // Call the appropriate function depending upon the current robot mode
+  if (mode == Mode::kDisabled) {
+    HAL_ObserveUserProgramDisabled();
+    DisabledPeriodic();
+    m_watchdog.AddEpoch("DisabledPeriodic()");
+  } else if (mode == Mode::kAutonomous) {
+    HAL_ObserveUserProgramAutonomous();
+    AutonomousPeriodic();
+    m_watchdog.AddEpoch("AutonomousPeriodic()");
+  } else if (mode == Mode::kTeleop) {
+    HAL_ObserveUserProgramTeleop();
+    TeleopPeriodic();
+    m_watchdog.AddEpoch("TeleopPeriodic()");
+  } else if (mode == Mode::kTest) {
+    HAL_ObserveUserProgramTest();
+    TestPeriodic();
+    m_watchdog.AddEpoch("TestPeriodic()");
+  }
+
+  RobotPeriodic();
+  m_watchdog.AddEpoch("RobotPeriodic()");
+
+  SmartDashboard::UpdateValues();
+  m_watchdog.AddEpoch("SmartDashboard::UpdateValues()");
+
+  if constexpr (IsSimulation()) {
+    HAL_SimPeriodicBefore();
+    SimulationPeriodic();
+    HAL_SimPeriodicAfter();
+    m_watchdog.AddEpoch("SimulationPeriodic()");
+  }
+
+  m_watchdog.Disable();
+
+  // Flush NetworkTables
+  if (m_ntFlushEnabled) {
+    wpi::nt::NetworkTableInstance::GetDefault().FlushLocal();
+  }
+
+  // Warn on loop time overruns
+  if (m_watchdog.IsExpired()) {
+    m_watchdog.PrintEpochs();
+  }
+}
+
+void IterativeRobotBase::PrintLoopOverrunMessage() {
+  WPILIB_ReportError(err::Error, "Loop time of {:.6f}s overrun",
+                     m_period.value());
+}
+
+void IterativeRobotBase::PrintWatchdogEpochs() {
+  m_watchdog.PrintEpochs();
+}

wpilibc/src/main/native/cpp/framework/RobotState.cpp

@@ -0,0 +1,33 @@
+// 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 "wpi/framework/RobotState.hpp"
+
+#include "wpi/driverstation/DriverStation.hpp"
+
+using namespace wpi;
+
+bool RobotState::IsDisabled() {
+  return DriverStation::IsDisabled();
+}
+
+bool RobotState::IsEnabled() {
+  return DriverStation::IsEnabled();
+}
+
+bool RobotState::IsEStopped() {
+  return DriverStation::IsEStopped();
+}
+
+bool RobotState::IsTeleop() {
+  return DriverStation::IsTeleop();
+}
+
+bool RobotState::IsAutonomous() {
+  return DriverStation::IsAutonomous();
+}
+
+bool RobotState::IsTest() {
+  return DriverStation::IsTest();
+}

wpilibc/src/main/native/cpp/framework/TimedRobot.cpp

@@ -0,0 +1,113 @@
+// 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 "wpi/framework/TimedRobot.hpp"
+
+#include <stdint.h>
+
+#include <cstdio>
+#include <utility>
+
+#include "wpi/hal/DriverStation.h"
+#include "wpi/hal/Notifier.h"
+#include "wpi/hal/UsageReporting.h"
+#include "wpi/system/Errors.hpp"
+
+using namespace wpi;
+
+void TimedRobot::StartCompetition() {
+  if constexpr (IsSimulation()) {
+    SimulationInit();
+  }
+
+  // Tell the DS that the robot is ready to be enabled
+  std::puts("\n********** Robot program startup complete **********");
+  HAL_ObserveUserProgramStarting();
+
+  // Loop forever, calling the appropriate mode-dependent function
+  while (true) {
+    // We don't have to check there's an element in the queue first because
+    // there's always at least one (the constructor adds one). It's reenqueued
+    // at the end of the loop.
+    auto callback = m_callbacks.pop();
+
+    int32_t status = 0;
+    HAL_UpdateNotifierAlarm(m_notifier, callback.expirationTime.count(),
+                            &status);
+    WPILIB_CheckErrorStatus(status, "UpdateNotifierAlarm");
+
+    std::chrono::microseconds currentTime{
+        HAL_WaitForNotifierAlarm(m_notifier, &status)};
+    if (currentTime.count() == 0 || status != 0) {
+      break;
+    }
+
+    m_loopStartTimeUs = RobotController::GetFPGATime();
+
+    callback.func();
+
+    // Increment the expiration time by the number of full periods it's behind
+    // plus one to avoid rapid repeat fires from a large loop overrun. We assume
+    // currentTime ≥ expirationTime rather than checking for it since the
+    // callback wouldn't be running otherwise.
+    callback.expirationTime +=
+        callback.period + (currentTime - callback.expirationTime) /
+                              callback.period * callback.period;
+    m_callbacks.push(std::move(callback));
+
+    // Process all other callbacks that are ready to run
+    while (m_callbacks.top().expirationTime <= currentTime) {
+      callback = m_callbacks.pop();
+
+      callback.func();
+
+      callback.expirationTime +=
+          callback.period + (currentTime - callback.expirationTime) /
+                                callback.period * callback.period;
+      m_callbacks.push(std::move(callback));
+    }
+  }
+}
+
+void TimedRobot::EndCompetition() {
+  int32_t status = 0;
+  HAL_StopNotifier(m_notifier, &status);
+}
+
+TimedRobot::TimedRobot(wpi::units::second_t period)
+    : IterativeRobotBase(period) {
+  m_startTime = std::chrono::microseconds{RobotController::GetFPGATime()};
+  AddPeriodic([=, this] { LoopFunc(); }, period);
+
+  int32_t status = 0;
+  m_notifier = HAL_InitializeNotifier(&status);
+  WPILIB_CheckErrorStatus(status, "InitializeNotifier");
+  HAL_SetNotifierName(m_notifier, "TimedRobot", &status);
+
+  HAL_ReportUsage("Framework", "TimedRobot");
+}
+
+TimedRobot::TimedRobot(wpi::units::hertz_t frequency)
+    : TimedRobot{1 / frequency} {}
+
+TimedRobot::~TimedRobot() {
+  if (m_notifier != HAL_kInvalidHandle) {
+    int32_t status = 0;
+    HAL_StopNotifier(m_notifier, &status);
+    WPILIB_ReportError(status, "StopNotifier");
+  }
+}
+
+uint64_t TimedRobot::GetLoopStartTime() {
+  return m_loopStartTimeUs;
+}
+
+void TimedRobot::AddPeriodic(std::function<void()> callback,
+                             wpi::units::second_t period,
+                             wpi::units::second_t offset) {
+  m_callbacks.emplace(
+      callback, m_startTime,
+      std::chrono::microseconds{static_cast<int64_t>(period.value() * 1e6)},
+      std::chrono::microseconds{static_cast<int64_t>(offset.value() * 1e6)});
+}

wpilibc/src/main/native/cpp/framework/TimesliceRobot.cpp

@@ -0,0 +1,27 @@
+// 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 "wpi/framework/TimesliceRobot.hpp"
+
+#include "wpi/system/Errors.hpp"
+
+using namespace wpi;
+
+TimesliceRobot::TimesliceRobot(wpi::units::second_t robotPeriodicAllocation,
+                               wpi::units::second_t controllerPeriod)
+    : m_nextOffset{robotPeriodicAllocation},
+      m_controllerPeriod{controllerPeriod} {}
+
+void TimesliceRobot::Schedule(std::function<void()> func,
+                              wpi::units::second_t allocation) {
+  if (m_nextOffset + allocation > m_controllerPeriod) {
+    throw WPILIB_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;
+}