copied everything over from 2012 and removed all of the actual robot code except the drivetrain stuff
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4078 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/aos/crio/controls/ControlsManager.cpp b/aos/crio/controls/ControlsManager.cpp
new file mode 100644
index 0000000..a37ec6f
--- /dev/null
+++ b/aos/crio/controls/ControlsManager.cpp
@@ -0,0 +1,53 @@
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "WPILib/Compressor.h"
+
+#include "aos/aos_core.h"
+#include "aos/crio/controls/ControlsManager.h"
+#include "aos/common/Configuration.h"
+#include "aos/crio/aos_ctdt.h"
+#include "aos/crio/motor_server/CRIOControlLoopRunner.h"
+#include "aos/crio/motor_server/MotorServer.h"
+
+namespace aos {
+namespace crio {
+
+// Everything gets an explicit Start call here before calling all of the init
+// functions because it means that all static variables will be initialized
+// before anything actually starts running. It also means that everything will
+// be initialized before any of the init functions start trying to register
+// themselves etc.
+void ControlsManager::StartCompetition() {
+ printf("aos::ControlsManager::RobotMain\n");
+ (new Compressor(14, 1))->Start();
+
+ logging::Start();
+ LOG(INFO, "logging started\n");
+
+ GetWatchdog().SetEnabled(false);
+ LOG(INFO, "disabled watchdog\n");
+
+ RegisterControlLoops();
+ LOG(INFO, "registered control loops\n");
+
+ // CRIOControlLoopRunner calls part of MotorServer, so MotorServer has to get
+ // initialized first.
+ MotorServer::Start();
+ LOG(INFO, "MotorServer started\n");
+ CRIOControlLoopRunner::Start();
+ LOG(INFO, "cRIO control loops started\n");
+
+ LOG(INFO, "calling init functions\n");
+ aos_call_init_functions();
+ LOG(INFO, "initialized\n");
+
+ // Wait forever so that this task doesn't end to avoid confusing any brittle
+ // FIRST code that might be hiding somewhere.
+ while (true) {
+ select(0, NULL, NULL, NULL, NULL);
+ }
+}
+
+} // namespace crio
+} // namespace aos
diff --git a/aos/crio/controls/ControlsManager.h b/aos/crio/controls/ControlsManager.h
new file mode 100644
index 0000000..986fe02
--- /dev/null
+++ b/aos/crio/controls/ControlsManager.h
@@ -0,0 +1,21 @@
+#include "WPILib/DriverStation.h"
+#include "WPILib/RobotBase.h"
+
+namespace aos {
+namespace crio {
+
+class ControlsManager : public RobotBase {
+ public:
+ // Gets called when it is time to register all the control loops.
+ virtual void RegisterControlLoops() = 0;
+ virtual void StartCompetition();
+ static inline ControlsManager &GetInstance() {
+ return *static_cast<ControlsManager *>(&RobotBase::getInstance());
+ }
+ inline DriverStation *GetDS() {
+ return m_ds;
+ }
+};
+
+} // namespace crio
+} // namespace aos
diff --git a/aos/crio/controls/JoyStickRead.cpp b/aos/crio/controls/JoyStickRead.cpp
new file mode 100644
index 0000000..1f5e4c7
--- /dev/null
+++ b/aos/crio/controls/JoyStickRead.cpp
@@ -0,0 +1,91 @@
+#include "WPILib/Task.h"
+#include "WPILib/Timer.h"
+
+#include "aos/aos_core.h"
+#include "aos/crio/controls/ControlsManager.h"
+#include "aos/common/network/SendSocket.h"
+#include "aos/common/messages/RobotState.q.h"
+
+namespace aos {
+namespace crio {
+
+class JoystickRead {
+ /*virtual void Disabled () {
+ int i = 0;
+ while (IsDisabled()) {
+ printf("Disabled! %d\n", i);
+ Wait(0.1);
+ i++;
+ }
+ printf("Done with disabled. %d\n", i);
+ }
+ virtual void Autonomous () {
+ int j = 0;
+ while (IsAutonomous()) {
+ printf("Autonomous! %d\n", j);
+ Wait(0.1);
+ //if (j > 5) {
+ //i(0);
+ //}
+ j ++;
+ }
+ printf("Done with autonomous. %d\n", j);
+ }
+ virtual void OperatorControl () {
+ int i = 0;
+ while (IsOperatorControl()) {
+ printf("Operator Control! %d\n", i);
+ Wait(0.1);
+ i ++;
+ }
+ printf("Done with operator control. %d\n", i);
+ }*/
+ public:
+ DriverStation *ds;
+ JoystickRead() {}
+ void Run() {
+ SendSocket sock(NetworkPort::kDS,
+ configuration::GetIPAddress(
+ configuration::NetworkDevice::kAtom));
+ FRCCommonControlData data;
+
+ ds = ControlsManager::GetInstance().GetDS();
+
+ while (true) {
+ // I checked, and this is done intelligently in WPILib.
+ ds->WaitForData();
+
+ robot_state.MakeWithBuilder().enabled(ds->IsEnabled())
+ .autonomous(ds->IsAutonomous()).team_id(ds->GetTeamNumber()).Send();
+ LOG(DEBUG, "sending joystick data\n");
+ data.enabled = ds->IsEnabled();
+ data.autonomous = ds->IsAutonomous();
+ data.fmsAttached = ds->IsFMSAttached();
+ SetStick(data.stick0Axes, 1);
+ SetStick(data.stick1Axes, 2);
+ SetStick(data.stick2Axes, 3);
+ SetStick(data.stick3Axes, 4);
+ data.stick0Buttons = ds->GetStickButtons(1);
+ data.stick1Buttons = ds->GetStickButtons(2);
+ data.stick2Buttons = ds->GetStickButtons(3);
+ data.stick3Buttons = ds->GetStickButtons(4);
+ data.teamID = ds->GetTeamNumber();
+ sock.Send(&data, sizeof(data));
+ }
+ }
+ void SetStick(int8_t axes[6], uint32_t stick) {
+ for (int i = 0; i < 6; ++i) {
+ double val = ds->GetStickAxis(stick, i + 1);
+ if (val < 0) {
+ axes[i] = (val * 128.0) + 0.5;
+ } else {
+ axes[i] = (val * 127.0) + 0.5;
+ }
+ }
+ }
+};
+
+} // namespace crio
+} // namespace aos
+
+AOS_RUN_FORK(aos::crio::JoystickRead, "JSR", 100)