mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
65 lines
1.7 KiB
C++
65 lines
1.7 KiB
C++
// 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 "wpi/xrp/XRPServo.hpp"
|
|
|
|
#include <map>
|
|
#include <numbers>
|
|
#include <set>
|
|
#include <string>
|
|
|
|
#include "wpi/system/Errors.hpp"
|
|
#include "wpi/units/angle.hpp"
|
|
|
|
using namespace wpi::xrp;
|
|
|
|
std::map<int, std::string> XRPServo::s_simDeviceMap = {
|
|
{4, "servo1"}, {5, "servo2"}, {6, "servo3"}, {7, "servo4"}};
|
|
|
|
std::set<int> XRPServo::s_registeredDevices = {};
|
|
|
|
void XRPServo::CheckDeviceAllocation(int deviceNum) {
|
|
if (s_simDeviceMap.count(deviceNum) == 0) {
|
|
throw WPILIB_MakeError(wpi::err::ChannelIndexOutOfRange, "Channel {}",
|
|
deviceNum);
|
|
}
|
|
|
|
if (s_registeredDevices.count(deviceNum) > 0) {
|
|
throw WPILIB_MakeError(wpi::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(wpi::units::radian_t angle) {
|
|
angle = std::clamp<wpi::units::radian_t>(angle, 0_deg, 180_deg);
|
|
double pos = angle.value() / std::numbers::pi;
|
|
|
|
if (m_simPosition) {
|
|
m_simPosition.Set(pos);
|
|
}
|
|
}
|
|
|
|
wpi::units::radian_t XRPServo::GetAngle() const {
|
|
if (m_simPosition) {
|
|
return wpi::units::radian_t{m_simPosition.Get() * std::numbers::pi};
|
|
}
|
|
|
|
return 90_deg;
|
|
}
|