diff --git a/README-RobotPy.md b/README-RobotPy.md index 2a6d1b81a8e..0136dbb21ca 100644 --- a/README-RobotPy.md +++ b/README-RobotPy.md @@ -58,6 +58,7 @@ bazel run //wpilibc:robotpy-wpilib-scan-headers ``` # wpi ExpansionHub = "wpi/ExpansionHub.hpp" +ExpansionHubCRServo = "wpi/ExpansionHubCRServo.hpp" ExpansionHubMotor = "wpi/ExpansionHubMotor.hpp" ExpansionHubPidConstants = "wpi/ExpansionHubPidConstants.hpp" ExpansionHubServo = "wpi/ExpansionHubServo.hpp" diff --git a/wpilibc/robotpy_pybind_build_info.bzl b/wpilibc/robotpy_pybind_build_info.bzl index 96b991a6307..c21da75fbdf 100644 --- a/wpilibc/robotpy_pybind_build_info.bzl +++ b/wpilibc/robotpy_pybind_build_info.bzl @@ -449,6 +449,16 @@ def wpilib_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], includ ("wpi::ExpansionHub", "wpi__ExpansionHub.hpp"), ], ), + struct( + class_name = "ExpansionHubCRServo", + yml_file = "semiwrap/ExpansionHubCRServo.yml", + header_root = "$(execpath :robotpy-native-wpilib.copy_headers)", + header_file = "$(execpath :robotpy-native-wpilib.copy_headers)/wpi/hardware/expansionhub/ExpansionHubCRServo.hpp", + tmpl_class_names = [], + trampolines = [ + ("wpi::ExpansionHubCRServo", "wpi__ExpansionHubCRServo.hpp"), + ], + ), struct( class_name = "ExpansionHubMotor", yml_file = "semiwrap/ExpansionHubMotor.yml", diff --git a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHub.cpp b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHub.cpp index 9730ef18800..a2c5fed690e 100644 --- a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHub.cpp +++ b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHub.cpp @@ -9,6 +9,7 @@ #include #include "wpi/hal/UsageReporting.hpp" +#include "wpi/hardware/expansionhub/ExpansionHubCRServo.hpp" #include "wpi/hardware/expansionhub/ExpansionHubMotor.hpp" #include "wpi/hardware/expansionhub/ExpansionHubServo.hpp" #include "wpi/nt/BooleanTopic.hpp" @@ -78,6 +79,10 @@ ExpansionHubMotor ExpansionHub::MakeMotor(int channel) { return ExpansionHubMotor{m_dataStore->m_usbId, channel}; } +ExpansionHubCRServo ExpansionHub::MakeCRServo(int channel) { + return ExpansionHubCRServo{m_dataStore->m_usbId, channel}; +} + bool ExpansionHub::IsHubConnected() const { return m_dataStore->m_hubConnectedSubscriber.Get(false); } diff --git a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubCRServo.cpp b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubCRServo.cpp new file mode 100644 index 00000000000..dab1635272c --- /dev/null +++ b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubCRServo.cpp @@ -0,0 +1,97 @@ +// 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/hardware/expansionhub/ExpansionHubCRServo.hpp" + +#include "wpi/system/Errors.hpp" +#include "wpi/system/SystemServer.hpp" + +using namespace wpi; + +ExpansionHubCRServo::ExpansionHubCRServo(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("ExHubCRServo[{}]", channel), "ExHubCRServo"); + + 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); +} + +ExpansionHubCRServo::~ExpansionHubCRServo() noexcept { + m_hub.UnreserveServo(m_channel); +} + +void ExpansionHubCRServo::SetThrottle(double value) { + value = std::clamp(value, -1.0, 1.0); + value = (value + 1.0) / 2.0; + if (m_reversed) { + value = 1.0 - value; + } + auto rawValue = ((value * GetFullRangeScaleFactor()) + m_minPwm); + SetPulseWidth(rawValue); +} + +void ExpansionHubCRServo::SetPulseWidth(wpi::units::microsecond_t pulseWidth) { + SetEnabled(true); + m_pulseWidthPublisher.Set(pulseWidth.value()); +} + +void ExpansionHubCRServo::SetEnabled(bool enabled) { + m_enabledPublisher.Set(enabled); +} + +void ExpansionHubCRServo::SetFramePeriod( + wpi::units::microsecond_t framePeriod) { + m_framePeriodPublisher.Set(framePeriod.value()); +} + +wpi::units::microsecond_t ExpansionHubCRServo::GetFullRangeScaleFactor() const { + return m_maxPwm - m_minPwm; +} + +void ExpansionHubCRServo::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 ExpansionHubCRServo::SetReversed(bool reversed) { + m_reversed = reversed; +} diff --git a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubMotor.cpp b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubMotor.cpp index 5913ea75cc0..49a12e6e5a8 100644 --- a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubMotor.cpp +++ b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubMotor.cpp @@ -30,7 +30,7 @@ ExpansionHubMotor::ExpansionHubMotor(int usbId, int channel) channel); } - m_hub.ReportUsage(fmt::format("ExHubServo[{}]", channel), "ExHubServo"); + m_hub.ReportUsage(fmt::format("ExHubMotor[{}]", channel), "ExHubMotor"); auto systemServer = SystemServer::GetSystemServer(); diff --git a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubServo.cpp b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubServo.cpp index 1f7d2bd278c..72838f01af6 100644 --- a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubServo.cpp +++ b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubServo.cpp @@ -54,12 +54,7 @@ ExpansionHubServo::~ExpansionHubServo() noexcept { m_hub.UnreserveServo(m_channel); } -void ExpansionHubServo::Set(double value) { - if (m_continuousMode) { - value = std::clamp(value, -1.0, 1.0); - value = (value + 1.0) / 2.0; - } - +void ExpansionHubServo::SetPosition(double value) { value = std::clamp(value, 0.0, 1.0); if (m_reversed) { value = 1.0 - value; @@ -72,7 +67,7 @@ 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()); + SetPosition((angle - m_minServoAngle) / GetServoAngleRange()); } void ExpansionHubServo::SetPulseWidth(wpi::units::microsecond_t pulseWidth) { @@ -106,7 +101,9 @@ void ExpansionHubServo::SetPWMRange(wpi::units::microsecond_t minPwm, m_maxPwm = maxPwm; } -void ExpansionHubServo::SetReversed(bool reversed) {} +void ExpansionHubServo::SetReversed(const bool reversed) { + m_reversed = reversed; +} void ExpansionHubServo::SetAngleRange(wpi::units::degree_t minAngle, wpi::units::degree_t maxAngle) { @@ -117,7 +114,3 @@ void ExpansionHubServo::SetAngleRange(wpi::units::degree_t minAngle, m_minServoAngle = minAngle; m_maxServoAngle = maxAngle; } - -void ExpansionHubServo::SetContinuousRotationMode(bool enable) { - m_continuousMode = enable; -} diff --git a/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHub.hpp b/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHub.hpp index 5a89d266bee..deded587ccf 100644 --- a/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHub.hpp +++ b/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHub.hpp @@ -12,6 +12,7 @@ namespace wpi { class ExpansionHubServo; class ExpansionHubMotor; +class ExpansionHubCRServo; /** This class controls a REV ExpansionHub plugged in over USB to Systemcore. */ class ExpansionHub { @@ -29,6 +30,7 @@ class ExpansionHub { friend class ExpansionHubServo; friend class ExpansionHubMotor; + friend class ExpansionHubCRServo; /** * Constructs a servo at the requested channel on this hub. @@ -40,6 +42,8 @@ class ExpansionHub { */ ExpansionHubServo MakeServo(int channel); + ExpansionHubCRServo MakeCRServo(int channel); + /** * Constructs a motor at the requested channel on this hub. * diff --git a/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubCRServo.hpp b/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubCRServo.hpp new file mode 100644 index 00000000000..7f52e734ad3 --- /dev/null +++ b/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubCRServo.hpp @@ -0,0 +1,105 @@ +// 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. + +#pragma once + +#include + +#include "wpi/hardware/expansionhub/ExpansionHub.hpp" +#include "wpi/nt/BooleanTopic.hpp" +#include "wpi/nt/IntegerTopic.hpp" +#include "wpi/units/time.hpp" + +namespace wpi { + +/** This class controls a specific servo in continuous rotation mode hooked up + * to an ExpansionHub. */ +class ExpansionHubCRServo { + public: + /** + * Constructs a continuous rotation servo at the requested channel on a + * specific USB port. + * + * @sa ExpansionHubServo for a servo mode, or non-continuous rotation servo + * @param usbId The USB port ID the hub is connected to + * @param channel The servo channel + */ + ExpansionHubCRServo(int usbId, int channel); + ~ExpansionHubCRServo() noexcept; + + /** + * Set the servo throttle. + * + * Throttle values range from -1.0 to 1.0 corresponding to full reverse to + * full forward. + * + * @param value Throttle from -1.0 to 1.0. + */ + void SetThrottle(double value); + + /** + * Sets the raw pulse width output on the servo. + * + * @param pulseWidth Pulse width + */ + void SetPulseWidth(wpi::units::microsecond_t pulseWidth); + + /** + * Sets if the servo output is enabled or not. Defaults to false. + * + * @param enabled True to enable, false to disable + */ + void SetEnabled(bool enabled); + + /** + * Sets the frame period for the servo. Defaults to 20ms. + * + * @param framePeriod The frame period + */ + void SetFramePeriod(wpi::units::microsecond_t framePeriod); + + /** + * Gets if the underlying ExpansionHub is connected. + * + * @return True if hub is connected, otherwise false + */ + bool IsHubConnected() const { return m_hub.IsHubConnected(); } + + /** + * Sets the PWM range for the servo. + * By default, this is 600 to 2400 microseconds. + * + * Maximum must be greater than minimum. + * + * @param minPwm Minimum PWM + * @param maxPwm Maximum PWM + */ + void SetPWMRange(wpi::units::microsecond_t minPwm, + wpi::units::microsecond_t maxPwm); + + /** + * Sets whether the servo is reversed. + * + * This will reverse SetThrottle. + * + * @param reversed True to reverse, false for normal + */ + void SetReversed(bool reversed); + + private: + wpi::units::microsecond_t GetFullRangeScaleFactor() const; + + ExpansionHub m_hub; + int m_channel; + + wpi::units::microsecond_t m_minPwm = 600_us; + wpi::units::microsecond_t m_maxPwm = 2400_us; + + bool m_reversed = false; + + wpi::nt::IntegerPublisher m_pulseWidthPublisher; + wpi::nt::IntegerPublisher m_framePeriodPublisher; + wpi::nt::BooleanPublisher m_enabledPublisher; +}; +} // namespace wpi diff --git a/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubServo.hpp b/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubServo.hpp index 4f26e3bf6f2..e857e14c134 100644 --- a/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubServo.hpp +++ b/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubServo.hpp @@ -14,12 +14,14 @@ namespace wpi { -/** This class controls a specific servo hooked up to an ExpansionHub. */ +/** This class controls a specific servo in positional/servo mode hooked up to + * an ExpansionHub. */ class ExpansionHubServo { public: /** * Constructs a servo at the requested channel on a specific USB port. * + * @sa ExpansionHubCRServo if the servo is in continuous rotation mode * @param usbId The USB port ID the hub is connected to * @param channel The servo channel */ @@ -34,16 +36,16 @@ class ExpansionHubServo { * * @param value Position from 0.0 to 1.0. */ - void Set(double value); + void SetPosition(double value); /** - * Sets the servo angle + * Sets the servo angle. * - * Servo angles range from 0 to 180 degrees. Use Set() with your own scaler - * for other angle ranges. + * Servo angles range defaults to 0 to 180 degrees, but can be changed with + * setAngleRange(). * - * @param angle Position in angle units. Will be scaled between 0 and 180 - * degrees + * @param angle Position in angle units. Will be clamped to be within the + * current angle range. */ void SetAngle(wpi::units::degree_t angle); @@ -76,7 +78,7 @@ class ExpansionHubServo { bool IsHubConnected() const { return m_hub.IsHubConnected(); } /** - * Sets the angle range for the setAngle call. + * Sets the angle range for the SetAngle call. * By default, this is 0 to 180 degrees. * * Maximum angle must be greater than minimum angle. @@ -102,22 +104,12 @@ class ExpansionHubServo { /** * Sets whether the servo is reversed. * - * This will reverse both Set() and SetAngle(). + * This will reverse both SetPosition() and SetAngle(). * * @param reversed True to reverse, false for normal */ void SetReversed(bool reversed); - /** - * Enables or disables continuous rotation mode. - * - * In continuous rotation mode, the servo will interpret - * Set() commands to between -1.0 and 1.0, instead of 0.0 to 1.0. - * - * @param enable True to enable continuous rotation mode, false to disable - */ - void SetContinuousRotationMode(bool enable); - private: wpi::units::microsecond_t GetFullRangeScaleFactor(); wpi::units::degree_t GetServoAngleRange(); @@ -132,7 +124,6 @@ class ExpansionHubServo { wpi::units::microsecond_t m_maxPwm = 2400_us; bool m_reversed = false; - bool m_continuousMode = false; wpi::nt::IntegerPublisher m_pulseWidthPublisher; wpi::nt::IntegerPublisher m_framePeriodPublisher; diff --git a/wpilibc/src/main/python/pyproject.toml b/wpilibc/src/main/python/pyproject.toml index d32ccdaaca9..b82e07fcc76 100644 --- a/wpilibc/src/main/python/pyproject.toml +++ b/wpilibc/src/main/python/pyproject.toml @@ -152,6 +152,7 @@ PWM = "wpi/hardware/discrete/PWM.hpp" # wpi/hardware/expansionhub ExpansionHub = "wpi/hardware/expansionhub/ExpansionHub.hpp" +ExpansionHubCRServo = "wpi/hardware/expansionhub/ExpansionHubCRServo.hpp" ExpansionHubMotor = "wpi/hardware/expansionhub/ExpansionHubMotor.hpp" ExpansionHubPositionConstants = "wpi/hardware/expansionhub/ExpansionHubPositionConstants.hpp" ExpansionHubServo = "wpi/hardware/expansionhub/ExpansionHubServo.hpp" diff --git a/wpilibc/src/main/python/semiwrap/ExpansionHub.yml b/wpilibc/src/main/python/semiwrap/ExpansionHub.yml index be33a7586f7..57e0361e6da 100644 --- a/wpilibc/src/main/python/semiwrap/ExpansionHub.yml +++ b/wpilibc/src/main/python/semiwrap/ExpansionHub.yml @@ -1,6 +1,7 @@ extra_includes: - wpi/hardware/expansionhub/ExpansionHubMotor.hpp - wpi/hardware/expansionhub/ExpansionHubServo.hpp +- wpi/hardware/expansionhub/ExpansionHubCRServo.hpp classes: wpi::ExpansionHub: @@ -11,6 +12,7 @@ classes: methods: ExpansionHub: MakeServo: + MakeCRServo: MakeMotor: IsHubConnected: GetUsbId: diff --git a/wpilibc/src/main/python/semiwrap/ExpansionHubCRServo.yml b/wpilibc/src/main/python/semiwrap/ExpansionHubCRServo.yml new file mode 100644 index 00000000000..8e1a9feeebf --- /dev/null +++ b/wpilibc/src/main/python/semiwrap/ExpansionHubCRServo.yml @@ -0,0 +1,11 @@ +classes: + wpi::ExpansionHubCRServo: + methods: + ExpansionHubCRServo: + SetThrottle: + SetPulseWidth: + SetEnabled: + SetFramePeriod: + IsHubConnected: + SetPWMRange: + SetReversed: diff --git a/wpilibc/src/main/python/semiwrap/ExpansionHubServo.yml b/wpilibc/src/main/python/semiwrap/ExpansionHubServo.yml index 1bbaffe3336..f30ff9572ee 100644 --- a/wpilibc/src/main/python/semiwrap/ExpansionHubServo.yml +++ b/wpilibc/src/main/python/semiwrap/ExpansionHubServo.yml @@ -2,7 +2,7 @@ classes: wpi::ExpansionHubServo: methods: ExpansionHubServo: - Set: + SetPosition: SetAngle: SetPulseWidth: SetEnabled: @@ -11,4 +11,3 @@ classes: SetAngleRange: SetPWMRange: SetReversed: - SetContinuousRotationMode: diff --git a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHub.java b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHub.java index 61605096c35..fdea4fb9fad 100644 --- a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHub.java +++ b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHub.java @@ -167,6 +167,18 @@ public ExpansionHubServo makeServo(int channel) { return new ExpansionHubServo(m_dataStore.m_usbId, channel); } + /** + * Constructs a continuous rotation servo at the requested channel on this hub. + * + *

Only a single instance of each servo per hub can be constructed at a time. + * + * @param channel The servo channel + * @return Continuous rotation servo object + */ + public ExpansionHubCRServo makeCRServo(int channel) { + return new ExpansionHubCRServo(m_dataStore.m_usbId, channel); + } + /** * Constructs a motor at the requested channel on this hub. * diff --git a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubCRServo.java b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubCRServo.java new file mode 100644 index 00000000000..7e1d831ac17 --- /dev/null +++ b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubCRServo.java @@ -0,0 +1,182 @@ +// 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. + +package org.wpilib.hardware.expansionhub; + +import static org.wpilib.units.Units.Microseconds; + +import org.wpilib.hardware.hal.util.AllocationException; +import org.wpilib.networktables.BooleanPublisher; +import org.wpilib.networktables.IntegerPublisher; +import org.wpilib.networktables.NetworkTableInstance; +import org.wpilib.networktables.PubSubOption; +import org.wpilib.system.SystemServer; +import org.wpilib.units.measure.Time; + +/** + * This class controls a specific servo servo in continuous rotation mode hooked up to an + * ExpansionHub. + */ +public class ExpansionHubCRServo implements AutoCloseable { + private ExpansionHub m_hub; + private final int m_channel; + + private boolean m_reversed; + + private final IntegerPublisher m_pulseWidthPublisher; + private final IntegerPublisher m_framePeriodPublisher; + private final BooleanPublisher m_enabledPublisher; + + private int m_minPwm = 600; + private int m_maxPwm = 2400; + + /** + * Constructs a continuous rotation servo at the requested channel on a specific USB port. + * + * @param usbId The USB port ID the hub is connected to + * @param channel The servo channel + */ + public ExpansionHubCRServo(int usbId, int channel) { + m_hub = new ExpansionHub(usbId); + m_channel = channel; + + if (!m_hub.checkServoChannel(channel)) { + m_hub.close(); + throw new IllegalArgumentException("Channel " + channel + " out of range"); + } + + if (!m_hub.checkAndReserveServo(channel)) { + m_hub.close(); + throw new AllocationException("ExpansionHub CR Servo already allocated"); + } + + m_hub.reportUsage("ExHubCRServo[" + channel + "]", "ExHubCRServo"); + + NetworkTableInstance systemServer = SystemServer.getSystemServer(); + + PubSubOption[] options = + new PubSubOption[] { + PubSubOption.SEND_ALL, PubSubOption.KEEP_DUPLICATES, PubSubOption.periodic(0.005) + }; + + m_pulseWidthPublisher = + systemServer + .getIntegerTopic("/rhsp/" + usbId + "/servo" + channel + "/pulseWidth") + .publish(options); + + m_pulseWidthPublisher.set(1500); + + m_framePeriodPublisher = + systemServer + .getIntegerTopic("/rhsp/" + usbId + "/servo" + channel + "/framePeriod") + .publish(options); + + m_framePeriodPublisher.set(20000); + + m_enabledPublisher = + systemServer + .getBooleanTopic("/rhsp/" + usbId + "/servo" + channel + "/enabled") + .publish(options); + } + + /** Closes a servo so another instance can be constructed. */ + @Override + public void close() { + m_hub.unreserveServo(m_channel); + m_hub.close(); + m_hub = null; + + m_pulseWidthPublisher.close(); + m_framePeriodPublisher.close(); + m_enabledPublisher.close(); + } + + /** + * Set the servo throttle. + * + *

Throttle values range from -1.0 to 1.0 corresponding to full reverse to full forward. + * + * @param value Throttle from -1.0 to 1.0. + */ + public void setThrottle(double value) { + value = Math.clamp(value, -1.0, 1.0); + value = (value + 1.0) / 2.0; + + if (m_reversed) { + value = 1.0 - value; + } + + int rawValue = (int) ((value * getFullRangeScaleFactor()) + m_minPwm); + + setPulseWidth(Microseconds.of(rawValue)); + } + + /** + * Sets the raw pulse width output on the servo. + * + * @param pulseWidth Pulse width + */ + public void setPulseWidth(Time pulseWidth) { + setEnabled(true); + m_pulseWidthPublisher.set((long) pulseWidth.in(Microseconds)); + } + + /** + * Sets if the servo output is enabled or not. Defaults to false. + * + * @param enabled True to enable, false to disable + */ + public void setEnabled(boolean enabled) { + m_enabledPublisher.set(enabled); + } + + /** + * Sets the frame period for the servo. Defaults to 20ms. + * + * @param framePeriod The frame period + */ + public void setFramePeriod(Time framePeriod) { + m_framePeriodPublisher.set((long) framePeriod.in(Microseconds)); + } + + /** + * Gets if the underlying ExpansionHub is connected. + * + * @return True if hub is connected, otherwise false + */ + public boolean isHubConnected() { + return m_hub.isHubConnected(); + } + + /** + * Sets whether the servo is reversed. + * + *

This will reverse setThrottle(). + * + * @param reversed True to reverse, false for normal + */ + public void setReversed(boolean reversed) { + m_reversed = reversed; + } + + /** + * Sets the PWM range for the servo. By default, this is 600 to 2400 microseconds. + * + *

Maximum must be greater than minimum. + * + * @param minPwm Minimum PWM + * @param maxPwm Maximum PWM + */ + public void setPWMRange(int minPwm, int maxPwm) { + if (maxPwm <= minPwm) { + throw new IllegalArgumentException("Maximum PWM must be greater than minimum PWM"); + } + m_minPwm = minPwm; + m_maxPwm = maxPwm; + } + + private double getFullRangeScaleFactor() { + return m_maxPwm - m_minPwm; + } +} diff --git a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubServo.java b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubServo.java index eec7a19b5d3..6b2422092e9 100644 --- a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubServo.java +++ b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubServo.java @@ -16,13 +16,12 @@ import org.wpilib.units.measure.Angle; import org.wpilib.units.measure.Time; -/** This class controls a specific servo hooked up to an ExpansionHub. */ +/** This class controls a specific servo in positional/servo mode hooked up to an ExpansionHub. */ public class ExpansionHubServo implements AutoCloseable { private ExpansionHub m_hub; private final int m_channel; private boolean m_reversed; - private boolean m_continuousMode; private final IntegerPublisher m_pulseWidthPublisher; private final IntegerPublisher m_framePeriodPublisher; @@ -86,17 +85,11 @@ public ExpansionHubServo(int usbId, int channel) { /** * Set the servo position. * - *

Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right. If - * continuous rotation mode is enabled, the range is -1.0 to 1.0. + *

Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right. * - * @param value Position from 0.0 to 1.0 (-1 to 1 in CR mode). + * @param value Position from 0.0 to 1.0. */ - public void set(double value) { - if (m_continuousMode) { - value = Math.clamp(value, -1.0, 1.0); - value = (value + 1.0) / 2.0; - } - + public void setPosition(double value) { value = Math.clamp(value, 0.0, 1.0); if (m_reversed) { @@ -114,7 +107,7 @@ public void set(double value) { * *

Servo angles range defaults to 0 to 180 degrees, but can be changed with setAngleRange(). * - * @param angle Position in angle units. Will be scaled between the current angle range. + * @param angle Position in angle units. Will be clamped to be within the current angle range. */ public void setAngle(Angle angle) { double dAngle = angle.in(Degrees); @@ -124,7 +117,7 @@ public void setAngle(Angle angle) { dAngle = m_maxServoAngle; } - set((dAngle - m_minServoAngle) / getServoAngleRange()); + setPosition((dAngle - m_minServoAngle) / getServoAngleRange()); } private double getFullRangeScaleFactor() { @@ -175,7 +168,7 @@ public boolean isHubConnected() { /** * Sets whether the servo is reversed. * - *

This will reverse both set() and setAngle(). + *

This will reverse both setPosition() and setAngle(). * * @param reversed True to reverse, false for normal */ @@ -215,18 +208,6 @@ public void setAngleRange(double minAngle, double maxAngle) { m_maxServoAngle = maxAngle; } - /** - * Enables or disables continuous rotation mode. - * - *

In continuous rotation mode, the servo will interpret Set() commands to between -1.0 and - * 1.0, instead of 0.0 to 1.0. - * - * @param enable True to enable continuous rotation mode, false to disable - */ - public void setContinuousRotationMode(boolean enable) { - m_continuousMode = enable; - } - /** Closes a servo so another instance can be constructed. */ @Override public void close() {