diff --git a/wpilibc/src/main/native/cpp/ExpansionHub.cpp b/wpilibc/src/main/native/cpp/ExpansionHub.cpp new file mode 100644 index 0000000000..ac8ad0d9cb --- /dev/null +++ b/wpilibc/src/main/native/cpp/ExpansionHub.cpp @@ -0,0 +1,108 @@ +// 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/ExpansionHub.h" + +#include + +#include +#include + +#include "frc/Errors.h" +#include "frc/ExpansionHubMotor.h" +#include "frc/ExpansionHubServo.h" +#include "frc/SystemServer.h" + +using namespace frc; + +wpi::mutex ExpansionHub::m_handleLock; +std::weak_ptr ExpansionHub::m_storeMap[4]; + +class ExpansionHub::DataStore { + public: + explicit DataStore(int usbId) : m_usbId{usbId} { + auto systemServer = SystemServer::GetSystemServer(); + + m_hubConnectedSubscriber = + systemServer.GetBooleanTopic(fmt::format("/rhsp/{}/connected", usbId)) + .Subscribe(false); + } + + DataStore(DataStore&) = delete; + DataStore(DataStore&&) = delete; + DataStore& operator=(DataStore&) = delete; + DataStore& operator=(DataStore&&) = delete; + + nt::BooleanSubscriber m_hubConnectedSubscriber; + + uint32_t m_reservedMotorMask{0}; + uint32_t m_reservedServoMask{0}; + wpi::mutex m_reservedLock; + + int m_usbId; +}; + +std::shared_ptr ExpansionHub::GetForUsbId(int usbId) { + FRC_AssertMessage(usbId >= 0 && usbId < NumUsbPorts, "USB {} out of range", + usbId); + std::scoped_lock lock{m_handleLock}; + std::weak_ptr& weakStore = m_storeMap[usbId]; + auto strongStore = weakStore.lock(); + if (!strongStore) { + strongStore = std::make_shared(usbId); + weakStore = strongStore; + } + return strongStore; +} + +ExpansionHub::ExpansionHub(int usbId) : m_dataStore{GetForUsbId(usbId)} {} + +ExpansionHubServo ExpansionHub::MakeServo(int channel) { + return ExpansionHubServo{m_dataStore->m_usbId, channel}; +} + +ExpansionHubMotor ExpansionHub::MakeMotor(int channel) { + return ExpansionHubMotor{m_dataStore->m_usbId, channel}; +} + +bool ExpansionHub::IsHubConnected() const { + return m_dataStore->m_hubConnectedSubscriber.Get(false); +} + +bool ExpansionHub::CheckAndReserveServo(int channel) { + int mask = 1 << channel; + std::scoped_lock lock{m_dataStore->m_reservedLock}; + if ((m_dataStore->m_reservedServoMask & mask) != 0) { + return false; + } + m_dataStore->m_reservedServoMask |= mask; + return true; +} + +void ExpansionHub::UnreserveServo(int channel) { + int mask = 1 << channel; + std::scoped_lock lock{m_dataStore->m_reservedLock}; + m_dataStore->m_reservedServoMask &= ~mask; +} + +bool ExpansionHub::CheckAndReserveMotor(int channel) { + int mask = 1 << channel; + std::scoped_lock lock{m_dataStore->m_reservedLock}; + if ((m_dataStore->m_reservedMotorMask & mask) != 0) { + return false; + } + m_dataStore->m_reservedMotorMask |= mask; + return true; +} + +void ExpansionHub::UnreserveMotor(int channel) { + int mask = 1 << channel; + std::scoped_lock lock{m_dataStore->m_reservedLock}; + m_dataStore->m_reservedMotorMask &= ~mask; +} + +void ExpansionHub::ReportUsage(std::string_view device, std::string_view data) { + HAL_ReportUsage( + fmt::format("ExpansionHub[{}]/{}", m_dataStore->m_usbId, device), data); +} diff --git a/wpilibc/src/main/native/cpp/ExpansionHubMotor.cpp b/wpilibc/src/main/native/cpp/ExpansionHubMotor.cpp new file mode 100644 index 0000000000..2ab5cd26ff --- /dev/null +++ b/wpilibc/src/main/native/cpp/ExpansionHubMotor.cpp @@ -0,0 +1,161 @@ +// 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/ExpansionHubMotor.h" + +#include "frc/Errors.h" +#include "frc/SystemServer.h" + +static constexpr int kPercentageMode = 0; +static constexpr int kVoltageMode = 1; +static constexpr int kPositionMode = 2; +static constexpr int kVelocityMode = 3; +static constexpr int kFollowerMode = 4; + +using namespace frc; + +ExpansionHubMotor::ExpansionHubMotor(int usbId, int channel) + : m_hub{usbId}, + m_channel{channel}, + m_velocityPidConstants{usbId, channel, true}, + m_positionPidConstants{usbId, channel, false} { + FRC_AssertMessage(channel >= 0 && channel < ExpansionHub::NumMotorPorts, + "ExHub Motor Channel {} out of range", channel); + + if (!m_hub.CheckAndReserveMotor(channel)) { + throw FRC_MakeError(err::ResourceAlreadyAllocated, "Channel {}", channel); + } + + m_hub.ReportUsage(fmt::format("ExHubServo[{}]", channel), "ExHubServo"); + + auto systemServer = SystemServer::GetSystemServer(); + + nt::PubSubOptions options; + options.sendAll = true; + options.keepDuplicates = true; + options.periodic = 0.005; + + m_encoderSubscriber = systemServer + .GetDoubleTopic(fmt::format( + "/rhsp/{}/motor{}/encoder", usbId, channel)) + .Subscribe(0, options); + m_encoderVelocitySubscriber = + systemServer + .GetDoubleTopic( + fmt::format("/rhsp/{}/motor{}/encoderVelocity", usbId, channel)) + .Subscribe(0, options); + m_currentSubscriber = systemServer + .GetDoubleTopic(fmt::format( + "/rhsp/{}/motor{}/current", usbId, channel)) + .Subscribe(0, options); + + m_setpointPublisher = systemServer + .GetDoubleTopic(fmt::format( + "/rhsp/{}/motor{}/setpoint", usbId, channel)) + .Publish(options); + + m_distancePerCountPublisher = + systemServer + .GetDoubleTopic( + fmt::format("/rhsp/{}/motor{}/distancePerCount", usbId, channel)) + .Publish(options); + + m_floatOn0Publisher = systemServer + .GetBooleanTopic(fmt::format( + "/rhsp/{}/motor{}/floatOn0", usbId, channel)) + .Publish(options); + m_enabledPublisher = systemServer + .GetBooleanTopic(fmt::format( + "/rhsp/{}/motor{}/enabled", usbId, channel)) + .Publish(options); + + m_modePublisher = + systemServer + .GetIntegerTopic(fmt::format("/rhsp/{}/motor{}/mode", usbId, channel)) + .Publish(options); + + m_reversedPublisher = systemServer + .GetBooleanTopic(fmt::format( + "/rhsp/{}/motor{}/reversed", usbId, channel)) + .Publish(options); + + m_resetEncoderPublisher = + systemServer + .GetBooleanTopic( + fmt::format("/rhsp/{}/motor{}/resetEncoder", usbId, channel)) + .Publish(options); +} + +ExpansionHubMotor::~ExpansionHubMotor() noexcept { + m_hub.UnreserveMotor(m_channel); +} + +void ExpansionHubMotor::SetPercentagePower(double power) { + m_modePublisher.Set(kPercentageMode); + m_setpointPublisher.Set(power); +} + +void ExpansionHubMotor::SetVoltage(units::volt_t voltage) { + m_modePublisher.Set(kVoltageMode); + m_setpointPublisher.Set(voltage.to()); +} + +void ExpansionHubMotor::SetPositionSetpoint(double setpoint) { + m_modePublisher.Set(kPositionMode); + m_setpointPublisher.Set(setpoint); +} + +void ExpansionHubMotor::SetVelocitySetpoint(double setpoint) { + m_modePublisher.Set(kVelocityMode); + m_setpointPublisher.Set(setpoint); +} + +void ExpansionHubMotor::SetEnabled(bool enabled) { + m_enabledPublisher.Set(enabled); +} + +void ExpansionHubMotor::SetFloatOn0(bool floatOn0) { + m_floatOn0Publisher.Set(floatOn0); +} + +units::ampere_t ExpansionHubMotor::GetCurrent() const { + return units::ampere_t{m_currentSubscriber.Get(0)}; +} + +void ExpansionHubMotor::SetDistancePerCount(double perCount) { + m_distancePerCountPublisher.Set(perCount); +} + +double ExpansionHubMotor::GetEncoderVelocity() const { + return m_encoderVelocitySubscriber.Get(0); +} + +double ExpansionHubMotor::GetEncoderPosition() const { + return m_encoderSubscriber.Get(0); +} + +void ExpansionHubMotor::SetReversed(bool reversed) { + m_reversedPublisher.Set(true); +} + +void ExpansionHubMotor::ResetEncoder() { + m_resetEncoderPublisher.Set(true); +} + +ExpansionHubPidConstants& ExpansionHubMotor::GetVelocityPidConstants() { + return m_velocityPidConstants; +} + +ExpansionHubPidConstants& ExpansionHubMotor::GetPositionPidConstants() { + return m_positionPidConstants; +} + +void ExpansionHubMotor::Follow(const ExpansionHubMotor& leader) { + if (m_hub.GetUsbId() != leader.m_hub.GetUsbId()) { + throw FRC_MakeError(err::InvalidParameter, + "Cannot follow motor on different hub"); + } + m_modePublisher.Set(kFollowerMode); + m_setpointPublisher.Set(leader.m_channel); +} diff --git a/wpilibc/src/main/native/cpp/ExpansionHubPidConstants.cpp b/wpilibc/src/main/native/cpp/ExpansionHubPidConstants.cpp new file mode 100644 index 0000000000..d02aace52f --- /dev/null +++ b/wpilibc/src/main/native/cpp/ExpansionHubPidConstants.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 "frc/ExpansionHubPidConstants.h" + +#include + +#include "frc/Errors.h" +#include "frc/SystemServer.h" + +using namespace frc; + +ExpansionHubPidConstants::ExpansionHubPidConstants(int usbId, int channel, + bool isVelocityPid) { + auto systemServer = SystemServer::GetSystemServer(); + + nt::PubSubOptions options; + options.sendAll = true; + options.keepDuplicates = true; + options.periodic = 0.005; + + std::string pidType = isVelocityPid ? "velocity" : "position"; + + m_pPublisher = systemServer + .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/kp", + usbId, channel, pidType)) + .Publish(options); + + m_iPublisher = systemServer + .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/ki", + usbId, channel, pidType)) + .Publish(options); + + m_dPublisher = systemServer + .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/kd", + usbId, channel, pidType)) + .Publish(options); + + m_aPublisher = systemServer + .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/ka", + usbId, channel, pidType)) + .Publish(options); + + m_vPublisher = systemServer + .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/kv", + usbId, channel, pidType)) + .Publish(options); + + m_sPublisher = systemServer + .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/ks", + usbId, channel, pidType)) + .Publish(options); + + m_continuousPublisher = + systemServer + .GetBooleanTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/continous", + usbId, channel, pidType)) + .Publish(options); + + m_continuousMinimumPublisher = + systemServer + .GetDoubleTopic( + fmt::format("/rhsp/{}/motor{}/pid/{}/continuousMinimum", usbId, + channel, pidType)) + .Publish(options); + + m_continuousMaximumPublisher = + systemServer + .GetDoubleTopic( + fmt::format("/rhsp/{}/motor{}/pid/{}/continousMaximum", usbId, + channel, pidType)) + .Publish(options); +} + +void ExpansionHubPidConstants::SetPID(double p, double i, double d) { + m_pPublisher.Set(p); + m_iPublisher.Set(i); + m_dPublisher.Set(d); +} + +void ExpansionHubPidConstants::SetFF(double s, double v, double a) { + m_sPublisher.Set(s); + m_vPublisher.Set(v); + m_aPublisher.Set(a); +} + +void ExpansionHubPidConstants::EnableContinousInput(double minimumInput, + double maximumInput) { + m_continuousMaximumPublisher.Set(maximumInput); + m_continuousMinimumPublisher.Set(minimumInput); + m_continuousPublisher.Set(true); +} + +void ExpansionHubPidConstants::DisableContinousInput() { + m_continuousPublisher.Set(false); +} diff --git a/wpilibc/src/main/native/cpp/ExpansionHubServo.cpp b/wpilibc/src/main/native/cpp/ExpansionHubServo.cpp new file mode 100644 index 0000000000..42499bc91b --- /dev/null +++ b/wpilibc/src/main/native/cpp/ExpansionHubServo.cpp @@ -0,0 +1,121 @@ +// 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/ExpansionHubServo.h" + +#include "frc/Errors.h" +#include "frc/SystemServer.h" + +using namespace frc; + +ExpansionHubServo::ExpansionHubServo(int usbId, int channel) + : m_hub{usbId}, m_channel{channel} { + FRC_AssertMessage(channel >= 0 && channel < ExpansionHub::NumServoPorts, + "ExHub Servo Channel {} out of range", channel); + + if (!m_hub.CheckAndReserveServo(channel)) { + throw FRC_MakeError(err::ResourceAlreadyAllocated, "Channel {}", channel); + } + + m_hub.ReportUsage(fmt::format("ExHubServo[{}]", channel), "ExHubServo"); + + auto systemServer = SystemServer::GetSystemServer(); + + 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(units::degree_t angle) { + angle = std::clamp(angle, m_minServoAngle, m_maxServoAngle); + + // NOLINTNEXTLINE(bugprone-integer-division) + Set((angle - m_minServoAngle) / GetServoAngleRange()); +} + +void ExpansionHubServo::SetPulseWidth(units::microsecond_t pulseWidth) { + m_pulseWidthPublisher.Set(pulseWidth.to()); +} + +void ExpansionHubServo::SetEnabled(bool enabled) { + m_enabledPublisher.Set(enabled); +} + +void ExpansionHubServo::SetFramePeriod(units::microsecond_t framePeriod) { + m_framePeriodPublisher.Set(framePeriod.to()); +} + +units::microsecond_t ExpansionHubServo::GetFullRangeScaleFactor() { + return m_maxPwm - m_minPwm; +} + +units::degree_t ExpansionHubServo::GetServoAngleRange() { + return m_maxServoAngle - m_minServoAngle; +} + +void ExpansionHubServo::SetPWMRange(units::microsecond_t minPwm, + units::microsecond_t maxPwm) { + if (maxPwm <= minPwm) { + throw FRC_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(units::degree_t minAngle, + units::degree_t maxAngle) { + if (maxAngle <= minAngle) { + throw FRC_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; +} diff --git a/wpilibc/src/main/native/include/frc/ExpansionHub.h b/wpilibc/src/main/native/include/frc/ExpansionHub.h new file mode 100644 index 0000000000..5e479cf6c1 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/ExpansionHub.h @@ -0,0 +1,92 @@ +// 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 + +#include + +namespace frc { +class ExpansionHubServo; +class ExpansionHubMotor; + +/** This class controls a REV ExpansionHub plugged in over USB to Systemcore. */ +class ExpansionHub { + public: + /** + * Constructs a new ExpansionHub for a given USB ID + * + * Multiple instances can be constructed, but will point to the same backing + * object with a ref count. + * + * @param usbId The USB Port ID the hub is plugged into. + */ + explicit ExpansionHub(int usbId); + ~ExpansionHub() noexcept = default; + + friend class ExpansionHubServo; + friend class ExpansionHubMotor; + + /** + * Constructs a 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 Servo object + */ + ExpansionHubServo MakeServo(int channel); + + /** + * Constructs a motor at the requested channel on this hub. + * + * Only a single instance of each motor per hub can be constructed at a time. + * + * @param channel The motor channel + * @return Motor object + */ + ExpansionHubMotor MakeMotor(int channel); + + /** + * Gets if the hub is currently connected over USB. + * + * @return True if hub connection, otherwise false + */ + bool IsHubConnected() const; + + /** + * Gets the USB ID of this hub. + * + * @return The USB ID + */ + int GetUsbId() const { return m_usbId; } + + static constexpr int NumUsbPorts = 4; + static constexpr int NumServoPorts = 6; + static constexpr int NumMotorPorts = 4; + + private: + bool CheckAndReserveServo(int channel); + void UnreserveServo(int channel); + + bool CheckAndReserveMotor(int channel); + void UnreserveMotor(int channel); + + void ReportUsage(std::string_view device, std::string_view data); + + class DataStore; + friend class DataStore; + + std::shared_ptr m_dataStore; + int m_usbId; + + static wpi::mutex m_handleLock; + static std::weak_ptr m_storeMap[4]; + + static std::shared_ptr GetForUsbId(int usbId); +}; + +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/ExpansionHubMotor.h b/wpilibc/src/main/native/include/frc/ExpansionHubMotor.h new file mode 100644 index 0000000000..329462a189 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/ExpansionHubMotor.h @@ -0,0 +1,174 @@ +// 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 +#include +#include +#include +#include +#include +#include + +#include "frc/ExpansionHub.h" +#include "frc/ExpansionHubPidConstants.h" + +namespace frc { + +/** This class controls a specific motor and encoder hooked up to an + * ExpansionHub. */ +class ExpansionHubMotor { + public: + /** + * Constructs a servo at the requested channel on a specific USB port. + * + * @param usbId The USB port ID the hub is connected to + * @param channel The motor channel + */ + ExpansionHubMotor(int usbId, int channel); + ~ExpansionHubMotor() noexcept; + + /** + * Sets the percentage power to run the motor at, between -1 and 1. + * + * @param power The power to drive the motor at + */ + void SetPercentagePower(double power); + + /** + * Sets the voltage to run the motor at. This value will be continously scaled + * to match the input voltage. + * + * @param voltage The voltage to drive the motor at + */ + void SetVoltage(units::volt_t voltage); + + /** + * Command the motor to drive to a specific position setpoint. This value will + * be scaled by SetDistancePerCount and influenced by the PID constants. + * + * @param setpoint The position setpoint to drive the motor to + */ + void SetPositionSetpoint(double setpoint); + + /** + * Command the motor to drive to a specific velocity setpoint. This value will + * be scaled by SetDistancePerCount and influenced by the PID constants. + * + * @param setpoint The velocity setpoint to drive the motor to + */ + void SetVelocitySetpoint(double setpoint); + + /** + * Sets if the motor output is enabled or not. Defaults to false. + * + * @param enabled True to enable, false to disable + */ + void SetEnabled(bool enabled); + + /** + * Sets if the motor should float or brake when 0 is commanded. Defaults to + * false. + * + * @param floatOn0 True to float when commanded 0, false to brake + */ + void SetFloatOn0(bool floatOn0); + + /** + * Gets the current being pulled by the motor. + * + * @return Motor current + */ + units::ampere_t GetCurrent() const; + + /** + * Sets the distance per count of the encoder. Used to scale encoder readings. + * + * @param perCount The distance moved per count of the encoder + */ + void SetDistancePerCount(double perCount); + + /** + * Gets the current velocity of the motor encoder. Scaled into + * distancePerCount units. + * + * @return Encoder velocity + */ + double GetEncoderVelocity() const; + + /** + * Gets the current position of the motor encoder. Scaled into + * distancePerCount units. + * + * @return Encoder position + */ + double GetEncoderPosition() const; + + /** + * Sets if the motor and encoder should be reversed. + * + * @param reversed True to reverse encoder, false otherwise + */ + void SetReversed(bool reversed); + + /** Reset the encoder count to 0. */ + void ResetEncoder(); + + /** + * Gets the PID constants object for velocity PID. + * + * @return Velocity PID constants object + */ + ExpansionHubPidConstants& GetVelocityPidConstants(); + + /** + * Gets the PID constants object for position PID. + * + * @return Position PID constants object + */ + ExpansionHubPidConstants& GetPositionPidConstants(); + + /** + * Gets if the underlying ExpansionHub is connected. + * + * @return True if hub is connected, otherwise false + */ + bool IsHubConnected() { return m_hub.IsHubConnected(); } + + /** + * Sets this motor to follow another motor on the same hub. + * + * This does not support following motors that are also followers. + * Additionally, the direction of both motors will be the same. + * + * @param leader The motor to follow + */ + void Follow(const ExpansionHubMotor& leader); + + private: + ExpansionHub m_hub; + int m_channel; + + nt::DoubleSubscriber m_encoderSubscriber; + nt::DoubleSubscriber m_encoderVelocitySubscriber; + nt::DoubleSubscriber m_currentSubscriber; + + nt::DoublePublisher m_setpointPublisher; + nt::BooleanPublisher m_floatOn0Publisher; + nt::BooleanPublisher m_enabledPublisher; + + nt::IntegerPublisher m_modePublisher; + + nt::BooleanPublisher m_reversedPublisher; + nt::BooleanPublisher m_resetEncoderPublisher; + + nt::DoublePublisher m_distancePerCountPublisher; + + ExpansionHubPidConstants m_velocityPidConstants; + ExpansionHubPidConstants m_positionPidConstants; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/ExpansionHubPidConstants.h b/wpilibc/src/main/native/include/frc/ExpansionHubPidConstants.h new file mode 100644 index 0000000000..4d9db27a87 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/ExpansionHubPidConstants.h @@ -0,0 +1,81 @@ +// 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 +#include + +namespace frc { +class ExpansionHubMotor; + +/** This class contains PID constants for an ExpansionHub motor. */ +class ExpansionHubPidConstants { + public: + /** + * Sets the PID Controller gain parameters. + * + * Sets the proportional, integral, and differential coefficients. + * + * @param p The proportional coefficient. Must be >= 0. + * @param i The integral coefficient. Must be >= 0. + * @param d The differential coefficient. Must be >= 0. + */ + void SetPID(double p, double i, double d); + + /** + * Sets the feed forward gains to the specified values. + * + * The units should be radians for angular systems and meters for linear + * systems. + * + * The PID control period is 10ms + * + * @param s The static gain in volts. + * @param v The velocity gain in V/(units/s). + * @param a The acceleration gain in V/(units/s²). + */ + void SetFF(double s, double v, double a); + + /** + * Enables continuous input. + * + * Rather then using the max and min input range as constraints, it considers + * them to be the same point and automatically calculates the shortest route + * to the setpoint. + * + * @param minimumInput The minimum value expected from the input. + * @param maximumInput The maximum value expected from the input. + */ + void EnableContinousInput(double minimumInput, double maximumInput); + + /** + * Disables continuous input. + */ + void DisableContinousInput(); + + ExpansionHubPidConstants(ExpansionHubPidConstants&) = delete; + ExpansionHubPidConstants& operator=(ExpansionHubPidConstants&) = delete; + + ExpansionHubPidConstants(ExpansionHubPidConstants&&) = default; + ExpansionHubPidConstants& operator=(ExpansionHubPidConstants&&) = default; + + friend class ExpansionHubMotor; + + private: + ExpansionHubPidConstants(int usbId, int channel, bool isVelocityPid); + + nt::DoublePublisher m_pPublisher; + nt::DoublePublisher m_iPublisher; + nt::DoublePublisher m_dPublisher; + nt::DoublePublisher m_sPublisher; + nt::DoublePublisher m_vPublisher; + nt::DoublePublisher m_aPublisher; + + nt::BooleanPublisher m_continuousPublisher; + nt::DoublePublisher m_continuousMinimumPublisher; + nt::DoublePublisher m_continuousMaximumPublisher; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/ExpansionHubServo.h b/wpilibc/src/main/native/include/frc/ExpansionHubServo.h new file mode 100644 index 0000000000..a3b138f968 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/ExpansionHubServo.h @@ -0,0 +1,140 @@ +// 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 +#include +#include +#include + +#include "frc/ExpansionHub.h" + +namespace frc { + +/** This class controls a specific servo hooked up to an ExpansionHub. */ +class ExpansionHubServo { + public: + /** + * Constructs a 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 + */ + ExpansionHubServo(int usbId, int channel); + ~ExpansionHubServo() noexcept; + + /** + * Set the servo position. + * + * 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. + */ + void Set(double value); + + /** + * Sets the servo angle + * + * Servo angles range from 0 to 180 degrees. Use Set() with your own scaler + * for other angle ranges. + * + * @param angle Position in angle units. Will be scaled between 0 and 180 + * degrees + */ + void SetAngle(units::degree_t angle); + + /** + * Sets the raw pulse width output on the servo. + * + * @param pulseWidth Pulse width + */ + void SetPulseWidth(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(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 angle range for the setAngle call. + * By default, this is 0 to 180 degrees. + * + * Maximum angle must be greater than minimum angle. + * + * @param minAngle Minimum angle + * @param maxAngle Maximum angle + */ + void SetAngleRange(units::degree_t minAngle, units::degree_t maxAngle); + + /** + * 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(units::microsecond_t minPwm, units::microsecond_t maxPwm); + + /** + * Sets whether the servo is reversed. + * + * This will reverse both Set() 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 SetContinousRotationMode(bool enable); + + private: + units::microsecond_t GetFullRangeScaleFactor(); + units::degree_t GetServoAngleRange(); + + ExpansionHub m_hub; + int m_channel; + + units::degree_t m_maxServoAngle = 180.0_deg; + units::degree_t m_minServoAngle = 0.0_deg; + + units::microsecond_t m_minPwm = 600_us; + units::microsecond_t m_maxPwm = 2400_us; + + bool m_reversed = false; + bool m_continousMode = false; + + nt::IntegerPublisher m_pulseWidthPublisher; + nt::IntegerPublisher m_framePeriodPublisher; + nt::BooleanPublisher m_enabledPublisher; +}; +} // namespace frc diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ExpansionHub.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ExpansionHub.java new file mode 100644 index 0000000000..563dbc5701 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ExpansionHub.java @@ -0,0 +1,187 @@ +// 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 edu.wpi.first.wpilibj; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.networktables.BooleanSubscriber; +import edu.wpi.first.networktables.NetworkTableInstance; + +/** This class controls a REV ExpansionHub plugged in over USB to Systemcore. */ +public class ExpansionHub implements AutoCloseable { + private static class DataStore implements AutoCloseable { + public final int m_usbId; + private int m_refCount; + private int m_reservedMotorMask; + private int m_reservedServoMask; + private final Object m_reserveLock = new Object(); + + private final BooleanSubscriber m_hubConnectedSubscriber; + + DataStore(int usbId) { + m_usbId = usbId; + m_storeMap[usbId] = this; + + NetworkTableInstance systemServer = SystemServer.getSystemServer(); + + m_hubConnectedSubscriber = + systemServer.getBooleanTopic("/rhsp/" + usbId + "/connected").subscribe(false); + } + + @Override + public void close() { + m_storeMap[m_usbId] = null; + } + + public void addRef() { + m_refCount++; + } + + public void removeRef() { + m_refCount--; + if (m_refCount == 0) { + this.close(); + } + } + } + + private static final DataStore[] m_storeMap = new DataStore[4]; + + private static void checkUsbId(int usbId) { + if (usbId < 0 || usbId > 3) { + throw new IllegalArgumentException("USB Port " + usbId + " out of range"); + } + } + + private static DataStore getForUsbId(int usbId) { + checkUsbId(usbId); + synchronized (m_storeMap) { + DataStore store = m_storeMap[usbId]; + if (store == null) { + store = new DataStore(usbId); + } + store.addRef(); + return store; + } + } + + private static void freeHub(DataStore store) { + synchronized (m_storeMap) { + store.removeRef(); + } + } + + private final DataStore m_dataStore; + + /** + * Constructs a new ExpansionHub for a given USB ID + * + *

Multiple instances can be constructed, but will point to the same backing object with a ref + * count. + * + * @param usbId The USB Port ID the hub is plugged into. + */ + public ExpansionHub(int usbId) { + m_dataStore = getForUsbId(usbId); + } + + /** + * Closes an ExpansionHub object. Will not close any other instances until the last instance is + * closed. + */ + @Override + public void close() { + freeHub(m_dataStore); + } + + boolean checkServoChannel(int channel) { + return channel >= 0 && channel < 6; + } + + boolean checkAndReserveServo(int channel) { + int mask = 1 << channel; + synchronized (m_dataStore.m_reserveLock) { + if ((m_dataStore.m_reservedServoMask & mask) != 0) { + return false; + } + m_dataStore.m_reservedServoMask |= mask; + return true; + } + } + + void unreserveServo(int channel) { + int mask = 1 << channel; + synchronized (m_dataStore.m_reserveLock) { + m_dataStore.m_reservedServoMask &= ~mask; + } + } + + boolean checkMotorChannel(int channel) { + return channel >= 0 && channel < 4; + } + + boolean checkAndReserveMotor(int channel) { + int mask = 1 << channel; + synchronized (m_dataStore.m_reserveLock) { + if ((m_dataStore.m_reservedMotorMask & mask) != 0) { + return false; + } + m_dataStore.m_reservedMotorMask |= mask; + return true; + } + } + + void unreserveMotor(int channel) { + int mask = 1 << channel; + synchronized (m_dataStore.m_reserveLock) { + m_dataStore.m_reservedMotorMask &= ~mask; + } + } + + void reportUsage(String device, String data) { + HAL.reportUsage("ExpansionHub[" + m_dataStore.m_usbId + "]/" + device, data); + } + + /** + * Constructs a 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 Servo object + */ + public ExpansionHubServo makeServo(int channel) { + return new ExpansionHubServo(m_dataStore.m_usbId, channel); + } + + /** + * Constructs a motor at the requested channel on this hub. + * + *

Only a single instance of each motor per hub can be constructed at a time. + * + * @param channel The motor channel + * @return Motor object + */ + public ExpansionHubMotor makeMotor(int channel) { + return new ExpansionHubMotor(m_dataStore.m_usbId, channel); + } + + /** + * Gets if the hub is currently connected over USB. + * + * @return True if hub connection, otherwise false + */ + public boolean isHubConnected() { + return m_dataStore.m_hubConnectedSubscriber.get(false); + } + + /** + * Gets the USB ID of this hub. + * + * @return The USB ID + */ + public int getUsbId() { + return m_dataStore.m_usbId; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ExpansionHubMotor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ExpansionHubMotor.java new file mode 100644 index 0000000000..d9d27ab8e7 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ExpansionHubMotor.java @@ -0,0 +1,308 @@ +// 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 edu.wpi.first.wpilibj; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.hal.util.AllocationException; +import edu.wpi.first.networktables.BooleanPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.IntegerPublisher; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; + +/** This class controls a specific motor and encoder hooked up to an ExpansionHub. */ +public class ExpansionHubMotor implements AutoCloseable { + private static final int kPercentageMode = 0; + private static final int kVoltageMode = 1; + private static final int kPositionMode = 2; + private static final int kVelocityMode = 3; + private static final int kFollowerMode = 4; + + private ExpansionHub m_hub; + private final int m_channel; + + private final DoubleSubscriber m_encoderSubscriber; + private final DoubleSubscriber m_encoderVelocitySubscriber; + private final DoubleSubscriber m_currentSubscriber; + + private final DoublePublisher m_setpointPublisher; + private final BooleanPublisher m_floatOn0Publisher; + private final BooleanPublisher m_enabledPublisher; + + private final IntegerPublisher m_modePublisher; + + private final BooleanPublisher m_reversedPublisher; + private final BooleanPublisher m_resetEncoderPublisher; + + private final DoublePublisher m_distancePerCountPublisher; + + private final ExpansionHubPidConstants m_velocityPidConstants; + private final ExpansionHubPidConstants m_positionPidConstants; + + /** + * Constructs a servo at the requested channel on a specific USB port. + * + * @param usbId The USB port ID the hub is connected to + * @param channel The motor channel + */ + public ExpansionHubMotor(int usbId, int channel) { + m_hub = new ExpansionHub(usbId); + m_channel = channel; + + if (!m_hub.checkMotorChannel(channel)) { + m_hub.close(); + throw new IllegalArgumentException("Channel " + channel + " out of range"); + } + + if (!m_hub.checkAndReserveMotor(channel)) { + m_hub.close(); + throw new AllocationException("ExpansionHub Motor already allocated"); + } + + m_hub.reportUsage("ExHubMotor[" + channel + "]", "ExHubMotor"); + + NetworkTableInstance systemServer = SystemServer.getSystemServer(); + + PubSubOption[] options = + new PubSubOption[] { + PubSubOption.sendAll(true), + PubSubOption.keepDuplicates(true), + PubSubOption.periodic(0.005) + }; + + m_encoderSubscriber = + systemServer + .getDoubleTopic("/rhsp/" + usbId + "/motor" + channel + "/encoder") + .subscribe(0, options); + m_encoderVelocitySubscriber = + systemServer + .getDoubleTopic("/rhsp/" + usbId + "/motor" + channel + "/encoderVelocity") + .subscribe(0, options); + m_currentSubscriber = + systemServer + .getDoubleTopic("/rhsp/" + usbId + "/motor" + channel + "/current") + .subscribe(0, options); + + m_setpointPublisher = + systemServer + .getDoubleTopic("/rhsp/" + usbId + "/motor" + channel + "/setpoint") + .publish(options); + + m_distancePerCountPublisher = + systemServer + .getDoubleTopic("/rhsp/" + usbId + "/motor" + channel + "/distancePerCount") + .publish(options); + + m_floatOn0Publisher = + systemServer + .getBooleanTopic("/rhsp/" + usbId + "/motor" + channel + "/floatOn0") + .publish(options); + m_enabledPublisher = + systemServer + .getBooleanTopic("/rhsp/" + usbId + "/motor" + channel + "/enabled") + .publish(options); + + m_modePublisher = + systemServer + .getIntegerTopic("/rhsp/" + usbId + "/motor" + channel + "/mode") + .publish(options); + + m_reversedPublisher = + systemServer + .getBooleanTopic("/rhsp/" + usbId + "/motor" + channel + "/reversed") + .publish(options); + + m_resetEncoderPublisher = + systemServer + .getBooleanTopic("/rhsp/" + usbId + "/motor" + channel + "/resetEncoder") + .publish(options); + + m_velocityPidConstants = new ExpansionHubPidConstants(usbId, channel, true); + m_positionPidConstants = new ExpansionHubPidConstants(usbId, channel, false); + } + + /** Closes a motor so another instance can be constructed. */ + @Override + public void close() { + m_hub.unreserveMotor(m_channel); + m_hub.close(); + m_hub = null; + + m_encoderSubscriber.close(); + m_encoderVelocitySubscriber.close(); + m_currentSubscriber.close(); + m_setpointPublisher.close(); + m_floatOn0Publisher.close(); + m_enabledPublisher.close(); + m_modePublisher.close(); + m_reversedPublisher.close(); + m_resetEncoderPublisher.close(); + m_distancePerCountPublisher.close(); + + m_velocityPidConstants.close(); + m_positionPidConstants.close(); + } + + /** + * Sets the percentage power to run the motor at, between -1 and 1. + * + * @param power The power to drive the motor at + */ + public void setPercentagePower(double power) { + m_modePublisher.set(kPercentageMode); + m_setpointPublisher.set(power); + } + + /** + * Sets the voltage to run the motor at. This value will be continously scaled to match the input + * voltage. + * + * @param voltage The voltage to drive the motor at + */ + public void setVoltage(Voltage voltage) { + m_modePublisher.set(kVoltageMode); + m_setpointPublisher.set(voltage.in(Volts)); + } + + /** + * Command the motor to drive to a specific position setpoint. This value will be scaled by + * setDistancePerCount and influenced by the PID constants. + * + * @param setpoint The position setpoint to drive the motor to + */ + public void setPositionSetpoint(double setpoint) { + m_modePublisher.set(kPositionMode); + m_setpointPublisher.set(setpoint); + } + + /** + * Command the motor to drive to a specific velocity setpoint. This value will be scaled by + * setDistancePerCount and influenced by the PID constants. + * + * @param setpoint The velocity setpoint to drive the motor to + */ + public void setVelocitySetpoint(double setpoint) { + m_modePublisher.set(kVelocityMode); + m_setpointPublisher.set(setpoint); + } + + /** + * Sets if the motor 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 if the motor should float or brake when 0 is commanded. Defaults to false. + * + * @param floatOn0 True to float when commanded 0, false to brake + */ + public void setFloatOn0(boolean floatOn0) { + m_floatOn0Publisher.set(floatOn0); + } + + /** + * Gets the current being pulled by the motor. + * + * @return Motor current + */ + public Current getCurrent() { + return Amps.of(m_currentSubscriber.get(0)); + } + + /** + * Sets the distance per count of the encoder. Used to scale encoder readings. + * + * @param perCount The distance moved per count of the encoder + */ + public void setDistancePerCount(double perCount) { + m_distancePerCountPublisher.set(perCount); + } + + /** + * Gets if the underlying ExpansionHub is connected. + * + * @return True if hub is connected, otherwise false + */ + public boolean isHubConnected() { + return m_hub.isHubConnected(); + } + + /** + * Gets the current velocity of the motor encoder. Scaled into distancePerCount units. + * + * @return Encoder velocity + */ + public double getEncoderVelocity() { + return m_encoderVelocitySubscriber.get(0); + } + + /** + * Gets the current position of the motor encoder. Scaled into distancePerCount units. + * + * @return Encoder position + */ + public double getEncoderPosition() { + return m_encoderSubscriber.get(0); + } + + /** + * Sets if the motor and encoder should be reversed. + * + * @param reversed True to reverse encoder, false otherwise + */ + public void setReversed(boolean reversed) { + m_reversedPublisher.set(reversed); + } + + /** Reset the encoder count to 0. */ + public void resetEncoder() { + m_resetEncoderPublisher.set(true); + } + + /** + * Gets the PID constants object for velocity PID. + * + * @return Velocity PID constants object + */ + public ExpansionHubPidConstants getVelocityPidConstants() { + return m_velocityPidConstants; + } + + /** + * Gets the PID constants object for position PID. + * + * @return Position PID constants object + */ + public ExpansionHubPidConstants getPositionPidConstants() { + return m_positionPidConstants; + } + + /** + * Sets this motor to follow another motor on the same hub. + * + *

This does not support following motors that are also followers. Additionally, the direction + * of both motors will be the same. + * + * @param leader The motor to follow + */ + public void follow(ExpansionHubMotor leader) { + requireNonNullParam(leader, "leader", "follow"); + if (leader.m_hub.getUsbId() != this.m_hub.getUsbId()) { + throw new IllegalArgumentException("Leader motor must be on the same hub as the follower"); + } + m_modePublisher.set(kFollowerMode); + m_setpointPublisher.set(leader.m_channel); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ExpansionHubPidConstants.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ExpansionHubPidConstants.java new file mode 100644 index 0000000000..968d45b9c6 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ExpansionHubPidConstants.java @@ -0,0 +1,166 @@ +// 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 edu.wpi.first.wpilibj; + +import edu.wpi.first.networktables.BooleanPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; + +/** This class contains PID constants for an ExpansionHub motor. */ +public class ExpansionHubPidConstants { + private final DoublePublisher m_pPublisher; + private final DoublePublisher m_iPublisher; + private final DoublePublisher m_dPublisher; + private final DoublePublisher m_sPublisher; + private final DoublePublisher m_vPublisher; + private final DoublePublisher m_aPublisher; + + private final BooleanPublisher m_continuousPublisher; + private final DoublePublisher m_continuousMinimumPublisher; + private final DoublePublisher m_continuousMaximumPublisher; + + ExpansionHubPidConstants(int hubNumber, int motorNumber, boolean isVelocityPid) { + NetworkTableInstance systemServer = SystemServer.getSystemServer(); + + PubSubOption[] options = + new PubSubOption[] { + PubSubOption.sendAll(true), + PubSubOption.keepDuplicates(true), + PubSubOption.periodic(0.005) + }; + + String pidType = isVelocityPid ? "velocity" : "position"; + + m_pPublisher = + systemServer + .getDoubleTopic( + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/kp") + .publish(options); + + m_iPublisher = + systemServer + .getDoubleTopic( + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/ki") + .publish(options); + + m_dPublisher = + systemServer + .getDoubleTopic( + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/kd") + .publish(options); + + m_aPublisher = + systemServer + .getDoubleTopic( + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/ka") + .publish(options); + + m_vPublisher = + systemServer + .getDoubleTopic( + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/kv") + .publish(options); + + m_sPublisher = + systemServer + .getDoubleTopic( + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/ks") + .publish(options); + + m_continuousPublisher = + systemServer + .getBooleanTopic( + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/continuous") + .publish(options); + + m_continuousMinimumPublisher = + systemServer + .getDoubleTopic( + "/rhsp/" + + hubNumber + + "/motor" + + motorNumber + + "/pid/" + + pidType + + "/continuousMinimum") + .publish(options); + + m_continuousMaximumPublisher = + systemServer + .getDoubleTopic( + "/rhsp/" + + hubNumber + + "/motor" + + motorNumber + + "/pid/" + + pidType + + "/continousMaximum") + .publish(options); + } + + /** + * Sets the PID Controller gain parameters. + * + *

Set the proportional, integral, and differential coefficients. + * + * @param p The proportional coefficient. + * @param i The integral coefficient. + * @param d The derivative coefficient. + */ + public void setPID(double p, double i, double d) { + m_pPublisher.set(p); + m_iPublisher.set(i); + m_dPublisher.set(d); + } + + /** + * Sets the feed forward gains to the specified values. + * + *

The units should be radians for angular systems and meters for linear systems. + * + *

The PID control period is 10ms + * + * @param s The static gain in volts. + * @param v The velocity gain in V/(units/s). + * @param a The acceleration gain in V/(units/s²). + */ + public void setFF(double s, double v, double a) { + m_sPublisher.set(s); + m_vPublisher.set(v); + m_aPublisher.set(a); + } + + /** + * Enables continuous input. + * + *

Rather then using the max and min input range as constraints, it considers them to be the + * same point and automatically calculates the shortest route to the setpoint. + * + * @param minimumInput The minimum value expected from the input. + * @param maximumInput The maximum value expected from the input. + */ + public void enableContinuousInput(double minimumInput, double maximumInput) { + m_continuousMaximumPublisher.set(maximumInput); + m_continuousMinimumPublisher.set(minimumInput); + m_continuousPublisher.set(true); + } + + /** Disable continous input mode. */ + public void disableContinuousInput() { + m_continuousPublisher.set(false); + } + + void close() { + m_iPublisher.close(); + m_dPublisher.close(); + m_sPublisher.close(); + m_vPublisher.close(); + m_aPublisher.close(); + m_continuousPublisher.close(); + m_continuousMinimumPublisher.close(); + m_continuousMaximumPublisher.close(); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ExpansionHubServo.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ExpansionHubServo.java new file mode 100644 index 0000000000..013778dfa1 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ExpansionHubServo.java @@ -0,0 +1,240 @@ +// 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 edu.wpi.first.wpilibj; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Microseconds; + +import edu.wpi.first.hal.util.AllocationException; +import edu.wpi.first.networktables.BooleanPublisher; +import edu.wpi.first.networktables.IntegerPublisher; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Time; + +/** This class controls a specific servo 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_continousMode; + + private final IntegerPublisher m_pulseWidthPublisher; + private final IntegerPublisher m_framePeriodPublisher; + private final BooleanPublisher m_enabledPublisher; + + private double m_maxServoAngle = 180.0; + private double m_minServoAngle; + + private int m_minPwm = 600; + private int m_maxPwm = 2400; + + /** + * Constructs a 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 ExpansionHubServo(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 Servo already allocated"); + } + + m_hub.reportUsage("ExHubServo[" + channel + "]", "ExHubServo"); + + NetworkTableInstance systemServer = SystemServer.getSystemServer(); + + PubSubOption[] options = + new PubSubOption[] { + PubSubOption.sendAll(true), + PubSubOption.keepDuplicates(true), + 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); + } + + /** + * 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. + * + * @param value Position from 0.0 to 1.0 (-1 to 1 in CR mode). + */ + public void set(double value) { + if (m_continousMode) { + value = Math.clamp(value, -1.0, 1.0); + value = (value + 1.0) / 2.0; + } + + value = Math.clamp(value, 0.0, 1.0); + + if (m_reversed) { + value = 1.0 - value; + } + + int rawValue = (int) ((value * getFullRangeScaleFactor()) + m_minPwm); + + m_pulseWidthPublisher.set(rawValue); + } + + /** + * Sets the servo angle + * + *

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. + */ + public void setAngle(Angle angle) { + double dAngle = angle.in(Degrees); + if (dAngle < m_minServoAngle) { + dAngle = m_minServoAngle; + } else if (dAngle > m_maxServoAngle) { + dAngle = m_maxServoAngle; + } + + set((dAngle - m_minServoAngle) / getServoAngleRange()); + } + + private double getFullRangeScaleFactor() { + return m_maxPwm - m_minPwm; + } + + private double getServoAngleRange() { + return m_maxServoAngle - m_minServoAngle; + } + + /** + * Sets the raw pulse width output on the servo. + * + * @param pulseWidth Pulse width + */ + public void setPulseWidth(Time pulseWidth) { + 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 both set() and setAngle(). + * + * @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; + } + + /** + * Sets the angle range for the setAngle call. By default, this is 0 to 180 degrees. + * + *

Maximum angle must be greater than minimum angle. + * + * @param minAngle Minimum angle + * @param maxAngle Maximum angle + */ + public void setAngleRange(double minAngle, double maxAngle) { + if (maxAngle <= minAngle) { + throw new IllegalArgumentException("Maximum angle must be greater than minimum angle"); + } + m_minServoAngle = minAngle; + 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 setContinousRotationMode(boolean enable) { + m_continousMode = enable; + } + + /** 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(); + } +}