Squashed 'third_party/allwpilib_2017/' content from commit 35ac87d
Change-Id: I7bb6f5556c30d3f5a092e68de0be9c710c60c9f4
git-subtree-dir: third_party/allwpilib_2017
git-subtree-split: 35ac87d6ff8b7f061c4f18c9ea316e5dccd4888a
diff --git a/wpilibc/athena/src/IterativeRobot.cpp b/wpilibc/athena/src/IterativeRobot.cpp
new file mode 100644
index 0000000..3b4e64f
--- /dev/null
+++ b/wpilibc/athena/src/IterativeRobot.cpp
@@ -0,0 +1,263 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "IterativeRobot.h"
+
+#include "DriverStation.h"
+#include "HAL/HAL.h"
+#include "LiveWindow/LiveWindow.h"
+#include "networktables/NetworkTable.h"
+
+using namespace frc;
+
+/**
+ * Provide an alternate "main loop" via StartCompetition().
+ *
+ * This specific StartCompetition() implements "main loop" behaviour synced with
+ * the DS packets.
+ */
+void IterativeRobot::StartCompetition() {
+ HAL_Report(HALUsageReporting::kResourceType_Framework,
+ HALUsageReporting::kFramework_Iterative);
+
+ LiveWindow* lw = LiveWindow::GetInstance();
+ // first and one-time initialization
+ NetworkTable::GetTable("LiveWindow")
+ ->GetSubTable("~STATUS~")
+ ->PutBoolean("LW Enabled", false);
+ RobotInit();
+
+ // Tell the DS that the robot is ready to be enabled
+ HAL_ObserveUserProgramStarting();
+
+ // loop forever, calling the appropriate mode-dependent function
+ lw->SetEnabled(false);
+ while (true) {
+ // wait for driver station data so the loop doesn't hog the CPU
+ m_ds.WaitForData();
+ // Call the appropriate function depending upon the current robot mode
+ if (IsDisabled()) {
+ // call DisabledInit() if we are now just entering disabled mode from
+ // either a different mode or from power-on
+ if (!m_disabledInitialized) {
+ lw->SetEnabled(false);
+ DisabledInit();
+ m_disabledInitialized = true;
+ // reset the initialization flags for the other modes
+ m_autonomousInitialized = false;
+ m_teleopInitialized = false;
+ m_testInitialized = false;
+ }
+ HAL_ObserveUserProgramDisabled();
+ DisabledPeriodic();
+ } else if (IsAutonomous()) {
+ // call AutonomousInit() if we are now just entering autonomous mode from
+ // either a different mode or from power-on
+ if (!m_autonomousInitialized) {
+ lw->SetEnabled(false);
+ AutonomousInit();
+ m_autonomousInitialized = true;
+ // reset the initialization flags for the other modes
+ m_disabledInitialized = false;
+ m_teleopInitialized = false;
+ m_testInitialized = false;
+ }
+ HAL_ObserveUserProgramAutonomous();
+ AutonomousPeriodic();
+ } else if (IsTest()) {
+ // call TestInit() if we are now just entering test mode from
+ // either a different mode or from power-on
+ if (!m_testInitialized) {
+ lw->SetEnabled(true);
+ TestInit();
+ m_testInitialized = true;
+ // reset the initialization flags for the other modes
+ m_disabledInitialized = false;
+ m_autonomousInitialized = false;
+ m_teleopInitialized = false;
+ }
+ HAL_ObserveUserProgramTest();
+ TestPeriodic();
+ } else {
+ // call TeleopInit() if we are now just entering teleop mode from
+ // either a different mode or from power-on
+ if (!m_teleopInitialized) {
+ lw->SetEnabled(false);
+ TeleopInit();
+ m_teleopInitialized = true;
+ // reset the initialization flags for the other modes
+ m_disabledInitialized = false;
+ m_autonomousInitialized = false;
+ m_testInitialized = false;
+ Scheduler::GetInstance()->SetEnabled(true);
+ }
+ HAL_ObserveUserProgramTeleop();
+ TeleopPeriodic();
+ }
+ RobotPeriodic();
+ }
+}
+
+/**
+ * Robot-wide initialization code should go here.
+ *
+ * Users should override this method for default Robot-wide initialization which
+ * will be called when the robot is first powered on. It will be called exactly
+ * one time.
+ *
+ * Warning: the Driver Station "Robot Code" light and FMS "Robot Ready"
+ * indicators will be off until RobotInit() exits. Code in RobotInit() that
+ * waits for enable will cause the robot to never indicate that the code is
+ * ready, causing the robot to be bypassed in a match.
+ */
+void IterativeRobot::RobotInit() {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for disabled mode should go here.
+ *
+ * Users should override this method for initialization code which will be
+ * called each time
+ * the robot enters disabled mode.
+ */
+void IterativeRobot::DisabledInit() {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for autonomous mode should go here.
+ *
+ * Users should override this method for initialization code which will be
+ * called each time the robot enters autonomous mode.
+ */
+void IterativeRobot::AutonomousInit() {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for teleop mode should go here.
+ *
+ * Users should override this method for initialization code which will be
+ * called each time the robot enters teleop mode.
+ */
+void IterativeRobot::TeleopInit() {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for test mode should go here.
+ *
+ * Users should override this method for initialization code which will be
+ * called each time the robot enters test mode.
+ */
+void IterativeRobot::TestInit() {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Periodic code for all modes should go here.
+ *
+ * This function is called each time a new packet is received from the driver
+ * station.
+ *
+ * Packets are received approximately every 20ms. Fixed loop timing is not
+ * guaranteed due to network timing variability and the function may not be
+ * called at all if the Driver Station is disconnected. For most use cases the
+ * variable timing will not be an issue. If your code does require guaranteed
+ * fixed periodic timing, consider using Notifier or PIDController instead.
+ */
+void IterativeRobot::RobotPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+}
+
+/**
+ * Periodic code for disabled mode should go here.
+ *
+ * Users should override this method for code which will be called each time a
+ * new packet is received from the driver station and the robot is in disabled
+ * mode.
+ *
+ * Packets are received approximately every 20ms. Fixed loop timing is not
+ * guaranteed due to network timing variability and the function may not be
+ * called at all if the Driver Station is disconnected. For most use cases the
+ * variable timing will not be an issue. If your code does require guaranteed
+ * fixed periodic timing, consider using Notifier or PIDController instead.
+ */
+void IterativeRobot::DisabledPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+}
+
+/**
+ * Periodic code for autonomous mode should go here.
+ *
+ * Users should override this method for code which will be called each time a
+ * new packet is received from the driver station and the robot is in autonomous
+ * mode.
+ *
+ * Packets are received approximately every 20ms. Fixed loop timing is not
+ * guaranteed due to network timing variability and the function may not be
+ * called at all if the Driver Station is disconnected. For most use cases the
+ * variable timing will not be an issue. If your code does require guaranteed
+ * fixed periodic timing, consider using Notifier or PIDController instead.
+ */
+void IterativeRobot::AutonomousPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+}
+
+/**
+ * Periodic code for teleop mode should go here.
+ *
+ * Users should override this method for code which will be called each time a
+ * new packet is received from the driver station and the robot is in teleop
+ * mode.
+ *
+ * Packets are received approximately every 20ms. Fixed loop timing is not
+ * guaranteed due to network timing variability and the function may not be
+ * called at all if the Driver Station is disconnected. For most use cases the
+ * variable timing will not be an issue. If your code does require guaranteed
+ * fixed periodic timing, consider using Notifier or PIDController instead.
+ */
+void IterativeRobot::TeleopPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+}
+
+/**
+ * Periodic code for test mode should go here.
+ *
+ * Users should override this method for code which will be called each time a
+ * new packet is received from the driver station and the robot is in test mode.
+ *
+ * Packets are received approximately every 20ms. Fixed loop timing is not
+ * guaranteed due to network timing variability and the function may not be
+ * called at all if the Driver Station is disconnected. For most use cases the
+ * variable timing will not be an issue. If your code does require guaranteed
+ * fixed periodic timing, consider using Notifier or PIDController instead.
+ */
+void IterativeRobot::TestPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+}