Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 1 | /*----------------------------------------------------------------------------*/ |
Austin Schuh | 1e69f94 | 2020-11-14 15:06:14 -0800 | [diff] [blame^] | 2 | /* Copyright (c) 2008-2020 FIRST. All Rights Reserved. */ |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 3 | /* 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 Schuh | 1e69f94 | 2020-11-14 15:06:14 -0800 | [diff] [blame^] | 20 | #include <wpimath/MathShared.h> |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 21 | |
| 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 | |
| 30 | typedef void (*SetCameraServerSharedFP)(frc::CameraServerShared* shared); |
| 31 | |
| 32 | using namespace frc; |
| 33 | |
| 34 | int 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 | |
| 45 | std::thread::id RobotBase::m_threadId; |
| 46 | |
| 47 | namespace { |
| 48 | class 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 Schuh | 1e69f94 | 2020-11-14 15:06:14 -0800 | [diff] [blame^] | 72 | class 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 Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 113 | } // namespace |
| 114 | |
| 115 | static 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 Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 133 | } 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 Schuh | 1e69f94 | 2020-11-14 15:06:14 -0800 | [diff] [blame^] | 146 | static void SetupMathShared() { |
| 147 | wpi::math::MathSharedStore::SetMathShared( |
| 148 | std::make_unique<WPILibMathShared>()); |
| 149 | } |
| 150 | |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 151 | bool RobotBase::IsEnabled() const { return m_ds.IsEnabled(); } |
| 152 | |
| 153 | bool RobotBase::IsDisabled() const { return m_ds.IsDisabled(); } |
| 154 | |
| 155 | bool RobotBase::IsAutonomous() const { return m_ds.IsAutonomous(); } |
| 156 | |
Austin Schuh | 1e69f94 | 2020-11-14 15:06:14 -0800 | [diff] [blame^] | 157 | bool RobotBase::IsAutonomousEnabled() const { |
| 158 | return m_ds.IsAutonomousEnabled(); |
| 159 | } |
| 160 | |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 161 | bool RobotBase::IsOperatorControl() const { return m_ds.IsOperatorControl(); } |
| 162 | |
Austin Schuh | 1e69f94 | 2020-11-14 15:06:14 -0800 | [diff] [blame^] | 163 | bool RobotBase::IsOperatorControlEnabled() const { |
| 164 | return m_ds.IsOperatorControlEnabled(); |
| 165 | } |
| 166 | |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 167 | bool RobotBase::IsTest() const { return m_ds.IsTest(); } |
| 168 | |
| 169 | bool RobotBase::IsNewDataAvailable() const { return m_ds.IsNewControlData(); } |
| 170 | |
| 171 | std::thread::id RobotBase::GetThreadId() { return m_threadId; } |
| 172 | |
| 173 | RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) { |
| 174 | m_threadId = std::this_thread::get_id(); |
| 175 | |
| 176 | SetupCameraServerShared(); |
Austin Schuh | 1e69f94 | 2020-11-14 15:06:14 -0800 | [diff] [blame^] | 177 | SetupMathShared(); |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 178 | |
| 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 | |
| 209 | RobotBase::RobotBase(RobotBase&&) noexcept |
| 210 | : m_ds(DriverStation::GetInstance()) {} |
| 211 | |
| 212 | RobotBase::~RobotBase() {} |
| 213 | |
| 214 | RobotBase& RobotBase::operator=(RobotBase&&) noexcept { return *this; } |