blob: 5f34e77a7418ec1a32e35ddeaf71f901af4a7a9b [file] [log] [blame]
Brian Silverman8fce7482020-01-05 13:18:21 -08001/*----------------------------------------------------------------------------*/
Austin Schuh1e69f942020-11-14 15:06:14 -08002/* Copyright (c) 2008-2020 FIRST. All Rights Reserved. */
Brian Silverman8fce7482020-01-05 13:18:21 -08003/* Open Source Software - may be modified and shared by FRC teams. The code */
4/* must be accompanied by the FIRST BSD license file in the root directory of */
5/* the project. */
6/*----------------------------------------------------------------------------*/
7
8#include "frc/RobotBase.h"
9
10#ifdef __FRC_ROBORIO__
11#include <dlfcn.h>
12#endif
13
14#include <cstdio>
15
16#include <cameraserver/CameraServerShared.h>
17#include <hal/FRCUsageReporting.h>
18#include <hal/HALBase.h>
19#include <networktables/NetworkTableInstance.h>
Austin Schuh1e69f942020-11-14 15:06:14 -080020#include <wpimath/MathShared.h>
Brian Silverman8fce7482020-01-05 13:18:21 -080021
22#include "WPILibVersion.h"
23#include "frc/DriverStation.h"
24#include "frc/RobotState.h"
25#include "frc/Utility.h"
26#include "frc/WPIErrors.h"
27#include "frc/livewindow/LiveWindow.h"
28#include "frc/smartdashboard/SmartDashboard.h"
29
30typedef void (*SetCameraServerSharedFP)(frc::CameraServerShared* shared);
31
32using namespace frc;
33
34int frc::RunHALInitialization() {
35 if (!HAL_Initialize(500, 0)) {
36 wpi::errs() << "FATAL ERROR: HAL could not be initialized\n";
37 return -1;
38 }
39 HAL_Report(HALUsageReporting::kResourceType_Language,
40 HALUsageReporting::kLanguage_CPlusPlus, 0, GetWPILibVersion());
41 wpi::outs() << "\n********** Robot program starting **********\n";
42 return 0;
43}
44
45std::thread::id RobotBase::m_threadId;
46
47namespace {
48class WPILibCameraServerShared : public frc::CameraServerShared {
49 public:
50 void ReportUsbCamera(int id) override {
51 HAL_Report(HALUsageReporting::kResourceType_UsbCamera, id);
52 }
53 void ReportAxisCamera(int id) override {
54 HAL_Report(HALUsageReporting::kResourceType_AxisCamera, id);
55 }
56 void ReportVideoServer(int id) override {
57 HAL_Report(HALUsageReporting::kResourceType_PCVideoServer, id);
58 }
59 void SetCameraServerError(const wpi::Twine& error) override {
60 wpi_setGlobalWPIErrorWithContext(CameraServerError, error);
61 }
62 void SetVisionRunnerError(const wpi::Twine& error) override {
63 wpi_setGlobalErrorWithContext(-1, error);
64 }
65 void ReportDriverStationError(const wpi::Twine& error) override {
66 DriverStation::ReportError(error);
67 }
68 std::pair<std::thread::id, bool> GetRobotMainThreadId() const override {
69 return std::make_pair(RobotBase::GetThreadId(), true);
70 }
71};
Austin Schuh1e69f942020-11-14 15:06:14 -080072class WPILibMathShared : public wpi::math::MathShared {
73 public:
74 void ReportError(const wpi::Twine& error) override {
75 DriverStation::ReportError(error);
76 }
77
78 void ReportUsage(wpi::math::MathUsageId id, int count) override {
79 switch (id) {
80 case wpi::math::MathUsageId::kKinematics_DifferentialDrive:
81 HAL_Report(HALUsageReporting::kResourceType_Kinematics,
82 HALUsageReporting::kKinematics_DifferentialDrive);
83 break;
84 case wpi::math::MathUsageId::kKinematics_MecanumDrive:
85 HAL_Report(HALUsageReporting::kResourceType_Kinematics,
86 HALUsageReporting::kKinematics_MecanumDrive);
87 break;
88 case wpi::math::MathUsageId::kKinematics_SwerveDrive:
89 HAL_Report(HALUsageReporting::kResourceType_Kinematics,
90 HALUsageReporting::kKinematics_SwerveDrive);
91 break;
92 case wpi::math::MathUsageId::kTrajectory_TrapezoidProfile:
93 HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, count);
94 break;
95 case wpi::math::MathUsageId::kFilter_Linear:
96 HAL_Report(HALUsageReporting::kResourceType_LinearFilter, count);
97 break;
98 case wpi::math::MathUsageId::kOdometry_DifferentialDrive:
99 HAL_Report(HALUsageReporting::kResourceType_Odometry,
100 HALUsageReporting::kOdometry_DifferentialDrive);
101 break;
102 case wpi::math::MathUsageId::kOdometry_SwerveDrive:
103 HAL_Report(HALUsageReporting::kResourceType_Odometry,
104 HALUsageReporting::kOdometry_SwerveDrive);
105 break;
106 case wpi::math::MathUsageId::kOdometry_MecanumDrive:
107 HAL_Report(HALUsageReporting::kResourceType_Odometry,
108 HALUsageReporting::kOdometry_MecanumDrive);
109 break;
110 }
111 }
112};
Brian Silverman8fce7482020-01-05 13:18:21 -0800113} // namespace
114
115static void SetupCameraServerShared() {
116#ifdef __FRC_ROBORIO__
117#ifdef DYNAMIC_CAMERA_SERVER
118#ifdef DYNAMIC_CAMERA_SERVER_DEBUG
119 auto cameraServerLib = dlopen("libcameraserverd.so", RTLD_NOW);
120#else
121 auto cameraServerLib = dlopen("libcameraserver.so", RTLD_NOW);
122#endif
123
124 if (!cameraServerLib) {
125 wpi::outs() << "Camera Server Library Not Found\n";
126 wpi::outs().flush();
127 return;
128 }
129 auto symbol = dlsym(cameraServerLib, "CameraServer_SetCameraServerShared");
130 if (symbol) {
131 auto setCameraServerShared = (SetCameraServerSharedFP)symbol;
132 setCameraServerShared(new WPILibCameraServerShared{});
Brian Silverman8fce7482020-01-05 13:18:21 -0800133 } else {
134 wpi::outs() << "Camera Server Shared Symbol Missing\n";
135 wpi::outs().flush();
136 }
137#else
138 CameraServer_SetCameraServerShared(new WPILibCameraServerShared{});
139#endif
140#else
141 wpi::outs() << "Not loading CameraServerShared\n";
142 wpi::outs().flush();
143#endif
144}
145
Austin Schuh1e69f942020-11-14 15:06:14 -0800146static void SetupMathShared() {
147 wpi::math::MathSharedStore::SetMathShared(
148 std::make_unique<WPILibMathShared>());
149}
150
Brian Silverman8fce7482020-01-05 13:18:21 -0800151bool RobotBase::IsEnabled() const { return m_ds.IsEnabled(); }
152
153bool RobotBase::IsDisabled() const { return m_ds.IsDisabled(); }
154
155bool RobotBase::IsAutonomous() const { return m_ds.IsAutonomous(); }
156
Austin Schuh1e69f942020-11-14 15:06:14 -0800157bool RobotBase::IsAutonomousEnabled() const {
158 return m_ds.IsAutonomousEnabled();
159}
160
Brian Silverman8fce7482020-01-05 13:18:21 -0800161bool RobotBase::IsOperatorControl() const { return m_ds.IsOperatorControl(); }
162
Austin Schuh1e69f942020-11-14 15:06:14 -0800163bool RobotBase::IsOperatorControlEnabled() const {
164 return m_ds.IsOperatorControlEnabled();
165}
166
Brian Silverman8fce7482020-01-05 13:18:21 -0800167bool RobotBase::IsTest() const { return m_ds.IsTest(); }
168
169bool RobotBase::IsNewDataAvailable() const { return m_ds.IsNewControlData(); }
170
171std::thread::id RobotBase::GetThreadId() { return m_threadId; }
172
173RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
174 m_threadId = std::this_thread::get_id();
175
176 SetupCameraServerShared();
Austin Schuh1e69f942020-11-14 15:06:14 -0800177 SetupMathShared();
Brian Silverman8fce7482020-01-05 13:18:21 -0800178
179 auto inst = nt::NetworkTableInstance::GetDefault();
180 inst.SetNetworkIdentity("Robot");
181#ifdef __FRC_ROBORIO__
182 inst.StartServer("/home/lvuser/networktables.ini");
183#else
184 inst.StartServer();
185#endif
186
187 SmartDashboard::init();
188
189 if (IsReal()) {
190 std::FILE* file = nullptr;
191 file = std::fopen("/tmp/frc_versions/FRC_Lib_Version.ini", "w");
192
193 if (file != nullptr) {
194 std::fputs("C++ ", file);
195 std::fputs(GetWPILibVersion(), file);
196 std::fclose(file);
197 }
198 }
199
200 // First and one-time initialization
201 inst.GetTable("LiveWindow")
202 ->GetSubTable(".status")
203 ->GetEntry("LW Enabled")
204 .SetBoolean(false);
205
206 LiveWindow::GetInstance()->SetEnabled(false);
207}
208
209RobotBase::RobotBase(RobotBase&&) noexcept
210 : m_ds(DriverStation::GetInstance()) {}
211
212RobotBase::~RobotBase() {}
213
214RobotBase& RobotBase::operator=(RobotBase&&) noexcept { return *this; }