[romi/xrp] Add Romi and XRP Vendordeps (#5644)

xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp

@@ -0,0 +1,87 @@
+// 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 "frc/xrp/XRPServo.h"
+
+#include <frc/Errors.h>
+
+using namespace frc;
+
+std::map<int, std::string> XRPServo::s_simDeviceMap = {{4, "servo1"},
+                                                       {5, "servo2"}};
+
+std::set<int> XRPServo::s_registeredDevices = {};
+
+void XRPServo::CheckDeviceAllocation(int deviceNum) {
+  if (s_simDeviceMap.count(deviceNum) == 0) {
+    throw FRC_MakeError(frc::err::ChannelIndexOutOfRange, "Channel {}",
+                        deviceNum);
+  }
+
+  if (s_registeredDevices.count(deviceNum) > 0) {
+    throw FRC_MakeError(frc::err::ResourceAlreadyAllocated, "Channel {}",
+                        deviceNum);
+  }
+
+  s_registeredDevices.insert(deviceNum);
+}
+
+XRPServo::XRPServo(int deviceNum) {
+  CheckDeviceAllocation(deviceNum);
+
+  m_deviceName = "XRPServo:" + s_simDeviceMap[deviceNum];
+  m_simDevice = hal::SimDevice(m_deviceName.c_str());
+
+  if (m_simDevice) {
+    m_simDevice.CreateBoolean("init", hal::SimDevice::kOutput, true);
+    m_simPosition =
+        m_simDevice.CreateDouble("position", hal::SimDevice::kOutput, 0.5);
+  }
+}
+
+void XRPServo::SetAngle(double angleDegrees) {
+  if (angleDegrees < 0.0) {
+    angleDegrees = 0.0;
+  }
+
+  if (angleDegrees > 180.0) {
+    angleDegrees = 180.0;
+  }
+
+  double pos = (angleDegrees / 180.0);
+
+  if (m_simPosition) {
+    m_simPosition.Set(pos);
+  }
+}
+
+double XRPServo::GetAngle() const {
+  if (m_simPosition) {
+    return m_simPosition.Get() * 180.0;
+  }
+
+  return 90.0;
+}
+
+void XRPServo::SetPosition(double pos) {
+  if (pos < 0.0) {
+    pos = 0.0;
+  }
+
+  if (pos > 1.0) {
+    pos = 1.0;
+  }
+
+  if (m_simPosition) {
+    m_simPosition.Set(pos);
+  }
+}
+
+double XRPServo::GetPosition() const {
+  if (m_simPosition) {
+    return m_simPosition.Get();
+  }
+
+  return 0.5;
+}