[wpilib] Move ExpansionHub to hardware/expansionhub (#8440)

These need to be moved out of the org.wpilib root for Java
modularization, and in general it's cleaner.

wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHub.hpp

@@ -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 <memory>
+#include <string_view>
+
+#include "wpi/util/mutex.hpp"
+
+namespace wpi {
+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<DataStore> m_dataStore;
+  int m_usbId;
+
+  static wpi::util::mutex m_handleLock;
+  static std::weak_ptr<DataStore> m_storeMap[4];
+
+  static std::shared_ptr<DataStore> GetForUsbId(int usbId);
+};
+
+}  // namespace wpi

wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubMotor.hpp

@@ -0,0 +1,173 @@
+// 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 <memory>
+
+#include "wpi/hardware/expansionhub/ExpansionHub.hpp"
+#include "wpi/hardware/expansionhub/ExpansionHubPidConstants.hpp"
+#include "wpi/nt/BooleanTopic.hpp"
+#include "wpi/nt/DoubleTopic.hpp"
+#include "wpi/nt/IntegerTopic.hpp"
+#include "wpi/units/angle.hpp"
+#include "wpi/units/current.hpp"
+#include "wpi/units/time.hpp"
+#include "wpi/units/voltage.hpp"
+
+namespace wpi {
+
+/** 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(wpi::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
+   */
+  wpi::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;
+
+  wpi::nt::DoubleSubscriber m_encoderSubscriber;
+  wpi::nt::DoubleSubscriber m_encoderVelocitySubscriber;
+  wpi::nt::DoubleSubscriber m_currentSubscriber;
+
+  wpi::nt::DoublePublisher m_setpointPublisher;
+  wpi::nt::BooleanPublisher m_floatOn0Publisher;
+  wpi::nt::BooleanPublisher m_enabledPublisher;
+
+  wpi::nt::IntegerPublisher m_modePublisher;
+
+  wpi::nt::BooleanPublisher m_reversedPublisher;
+  wpi::nt::BooleanPublisher m_resetEncoderPublisher;
+
+  wpi::nt::DoublePublisher m_distancePerCountPublisher;
+
+  ExpansionHubPidConstants m_velocityPidConstants;
+  ExpansionHubPidConstants m_positionPidConstants;
+};
+}  // namespace wpi

wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubPidConstants.hpp

@@ -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 "wpi/nt/BooleanTopic.hpp"
+#include "wpi/nt/DoubleTopic.hpp"
+#include "wpi/nt/IntegerTopic.hpp"
+
+namespace wpi {
+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);
+
+  wpi::nt::DoublePublisher m_pPublisher;
+  wpi::nt::DoublePublisher m_iPublisher;
+  wpi::nt::DoublePublisher m_dPublisher;
+  wpi::nt::DoublePublisher m_sPublisher;
+  wpi::nt::DoublePublisher m_vPublisher;
+  wpi::nt::DoublePublisher m_aPublisher;
+
+  wpi::nt::BooleanPublisher m_continuousPublisher;
+  wpi::nt::DoublePublisher m_continuousMinimumPublisher;
+  wpi::nt::DoublePublisher m_continuousMaximumPublisher;
+};
+}  // namespace wpi

wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubServo.hpp

@@ -0,0 +1,141 @@
+// 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 <memory>
+
+#include "wpi/hardware/expansionhub/ExpansionHub.hpp"
+#include "wpi/nt/BooleanTopic.hpp"
+#include "wpi/nt/IntegerTopic.hpp"
+#include "wpi/units/angle.hpp"
+#include "wpi/units/time.hpp"
+
+namespace wpi {
+
+/** 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(wpi::units::degree_t angle);
+
+  /**
+   * 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 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(wpi::units::degree_t minAngle,
+                     wpi::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(wpi::units::microsecond_t minPwm,
+                   wpi::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:
+  wpi::units::microsecond_t GetFullRangeScaleFactor();
+  wpi::units::degree_t GetServoAngleRange();
+
+  ExpansionHub m_hub;
+  int m_channel;
+
+  wpi::units::degree_t m_maxServoAngle = 180.0_deg;
+  wpi::units::degree_t m_minServoAngle = 0.0_deg;
+
+  wpi::units::microsecond_t m_minPwm = 600_us;
+  wpi::units::microsecond_t m_maxPwm = 2400_us;
+
+  bool m_reversed = false;
+  bool m_continousMode = false;
+
+  wpi::nt::IntegerPublisher m_pulseWidthPublisher;
+  wpi::nt::IntegerPublisher m_framePeriodPublisher;
+  wpi::nt::BooleanPublisher m_enabledPublisher;
+};
+}  // namespace wpi