mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
122 lines
3.6 KiB
C++
122 lines
3.6 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/ExpansionHubServo.hpp"
|
|
|
|
#include "wpi/system/Errors.hpp"
|
|
#include "wpi/system/SystemServer.hpp"
|
|
|
|
using namespace wpi;
|
|
|
|
ExpansionHubServo::ExpansionHubServo(int usbId, int channel)
|
|
: m_hub{usbId}, m_channel{channel} {
|
|
WPILIB_AssertMessage(channel >= 0 && channel < ExpansionHub::NumServoPorts,
|
|
"ExHub Servo Channel {} out of range", channel);
|
|
|
|
if (!m_hub.CheckAndReserveServo(channel)) {
|
|
throw WPILIB_MakeError(err::ResourceAlreadyAllocated, "Channel {}", channel);
|
|
}
|
|
|
|
m_hub.ReportUsage(fmt::format("ExHubServo[{}]", channel), "ExHubServo");
|
|
|
|
auto systemServer = SystemServer::GetSystemServer();
|
|
|
|
wpi::nt::PubSubOptions options;
|
|
options.sendAll = true;
|
|
options.keepDuplicates = true;
|
|
options.periodic = 0.005;
|
|
|
|
m_pulseWidthPublisher =
|
|
systemServer
|
|
.GetIntegerTopic(
|
|
fmt::format("/rhsp/{}/servo{}/pulseWidth", usbId, channel))
|
|
.Publish(options);
|
|
|
|
m_pulseWidthPublisher.Set(1500);
|
|
|
|
m_framePeriodPublisher =
|
|
systemServer
|
|
.GetIntegerTopic(
|
|
fmt::format("/rhsp/{}/servo{}/framePeriod", usbId, channel))
|
|
.Publish(options);
|
|
|
|
m_framePeriodPublisher.Set(20000);
|
|
|
|
m_enabledPublisher = systemServer
|
|
.GetBooleanTopic(fmt::format(
|
|
"/rhsp/{}/servo{}/enabled", usbId, channel))
|
|
.Publish(options);
|
|
}
|
|
|
|
ExpansionHubServo::~ExpansionHubServo() noexcept {
|
|
m_hub.UnreserveServo(m_channel);
|
|
}
|
|
|
|
void ExpansionHubServo::Set(double value) {
|
|
if (m_continousMode) {
|
|
value = std::clamp(value, -1.0, 1.0);
|
|
value = (value + 1.0) / 2.0;
|
|
}
|
|
|
|
value = std::clamp(value, 0.0, 1.0);
|
|
if (m_reversed) {
|
|
value = 1.0 - value;
|
|
}
|
|
auto rawValue = ((value * GetFullRangeScaleFactor()) + m_minPwm);
|
|
SetPulseWidth(rawValue);
|
|
}
|
|
|
|
void ExpansionHubServo::SetAngle(wpi::units::degree_t angle) {
|
|
angle = std::clamp(angle, m_minServoAngle, m_maxServoAngle);
|
|
|
|
// NOLINTNEXTLINE(bugprone-integer-division)
|
|
Set((angle - m_minServoAngle) / GetServoAngleRange());
|
|
}
|
|
|
|
void ExpansionHubServo::SetPulseWidth(wpi::units::microsecond_t pulseWidth) {
|
|
m_pulseWidthPublisher.Set(pulseWidth.to<double>());
|
|
}
|
|
|
|
void ExpansionHubServo::SetEnabled(bool enabled) {
|
|
m_enabledPublisher.Set(enabled);
|
|
}
|
|
|
|
void ExpansionHubServo::SetFramePeriod(wpi::units::microsecond_t framePeriod) {
|
|
m_framePeriodPublisher.Set(framePeriod.to<double>());
|
|
}
|
|
|
|
wpi::units::microsecond_t ExpansionHubServo::GetFullRangeScaleFactor() {
|
|
return m_maxPwm - m_minPwm;
|
|
}
|
|
|
|
wpi::units::degree_t ExpansionHubServo::GetServoAngleRange() {
|
|
return m_maxServoAngle - m_minServoAngle;
|
|
}
|
|
|
|
void ExpansionHubServo::SetPWMRange(wpi::units::microsecond_t minPwm,
|
|
wpi::units::microsecond_t maxPwm) {
|
|
if (maxPwm <= minPwm) {
|
|
throw WPILIB_MakeError(err::ParameterOutOfRange,
|
|
"Max PWM must be greater than Min PWM");
|
|
}
|
|
m_minPwm = minPwm;
|
|
m_maxPwm = maxPwm;
|
|
}
|
|
|
|
void ExpansionHubServo::SetReversed(bool reversed) {}
|
|
|
|
void ExpansionHubServo::SetAngleRange(wpi::units::degree_t minAngle,
|
|
wpi::units::degree_t maxAngle) {
|
|
if (maxAngle <= minAngle) {
|
|
throw WPILIB_MakeError(err::ParameterOutOfRange,
|
|
"Max angle must be greater than Min angle");
|
|
}
|
|
m_minServoAngle = minAngle;
|
|
m_maxServoAngle = maxAngle;
|
|
}
|
|
|
|
void ExpansionHubServo::SetContinousRotationMode(bool enable) {
|
|
m_continousMode = enable;
|
|
}
|