[hal, wpilib] PWM Rewrite (#7845)

The HAL will only contain the output period and the raw microseconds. Higher level things such as SimDevice can handle everything else.

wpilibc/src/main/native/cpp/PWM.cpp

@@ -31,11 +31,7 @@ PWM::PWM(int channel, bool registerSendable) {
 
   m_channel = channel;
 
-  HAL_SetPWMDisabled(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Channel {}", channel);
-  status = 0;
-  HAL_SetPWMEliminateDeadband(m_handle, false, &status);
-  FRC_CheckErrorStatus(status, "Channel {}", channel);
+  SetDisabled();
 
   HAL_ReportUsage("IO", channel, "PWM");
   if (registerSendable) {
@@ -45,9 +41,7 @@ PWM::PWM(int channel, bool registerSendable) {
 
 PWM::~PWM() {
   if (m_handle != HAL_kInvalidHandle) {
-    int32_t status = 0;
-    HAL_SetPWMDisabled(m_handle, &status);
-    FRC_ReportError(status, "Channel {}", m_channel);
+    SetDisabled();
   }
 }
 
@@ -65,121 +59,48 @@ units::microsecond_t PWM::GetPulseTime() const {
   return units::microsecond_t{value};
 }
 
-void PWM::SetPosition(double pos) {
-  int32_t status = 0;
-  HAL_SetPWMPosition(m_handle, pos, &status);
-  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
-}
-
-double PWM::GetPosition() const {
-  int32_t status = 0;
-  double position = HAL_GetPWMPosition(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
-  return position;
-}
-
-void PWM::SetSpeed(double speed) {
-  int32_t status = 0;
-  HAL_SetPWMSpeed(m_handle, speed, &status);
-  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
-}
-
-double PWM::GetSpeed() const {
-  int32_t status = 0;
-  double speed = HAL_GetPWMSpeed(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
-  return speed;
-}
-
 void PWM::SetDisabled() {
   int32_t status = 0;
-  HAL_SetPWMDisabled(m_handle, &status);
+  HAL_SetPWMPulseTimeMicroseconds(m_handle, 0, &status);
   FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
-void PWM::SetPeriodMultiplier(PeriodMultiplier mult) {
+void PWM::SetOutputPeriod(OutputPeriod mult) {
   int32_t status = 0;
 
   switch (mult) {
-    case kPeriodMultiplier_4X:
-      HAL_SetPWMPeriodScale(m_handle, 3,
-                            &status);  // Squelch 3 out of 4 outputs
+    case kOutputPeriod_20Ms:
+      HAL_SetPWMOutputPeriod(m_handle, 3,
+                             &status);  // Squelch 3 out of 4 outputs
       break;
-    case kPeriodMultiplier_2X:
-      HAL_SetPWMPeriodScale(m_handle, 1,
-                            &status);  // Squelch 1 out of 2 outputs
+    case kOutputPeriod_10Ms:
+      HAL_SetPWMOutputPeriod(m_handle, 1,
+                             &status);  // Squelch 1 out of 2 outputs
       break;
-    case kPeriodMultiplier_1X:
-      HAL_SetPWMPeriodScale(m_handle, 0, &status);  // Don't squelch any outputs
+    case kOutputPeriod_5Ms:
+      HAL_SetPWMOutputPeriod(m_handle, 0,
+                             &status);  // Don't squelch any outputs
       break;
     default:
-      throw FRC_MakeError(err::InvalidParameter, "PeriodMultiplier value {}",
+      throw FRC_MakeError(err::InvalidParameter, "OutputPeriod value {}",
                           static_cast<int>(mult));
   }
 
   FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
-void PWM::SetZeroLatch() {
-  int32_t status = 0;
-  HAL_LatchPWMZero(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
-}
-
-void PWM::EnableDeadbandElimination(bool eliminateDeadband) {
-  int32_t status = 0;
-  HAL_SetPWMEliminateDeadband(m_handle, eliminateDeadband, &status);
-  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
-}
-
-void PWM::SetBounds(units::microsecond_t max, units::microsecond_t deadbandMax,
-                    units::microsecond_t center,
-                    units::microsecond_t deadbandMin,
-                    units::microsecond_t min) {
-  int32_t status = 0;
-  HAL_SetPWMConfigMicroseconds(m_handle, max.value(), deadbandMax.value(),
-                               center.value(), deadbandMin.value(), min.value(),
-                               &status);
-  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
-}
-
-void PWM::GetBounds(units::microsecond_t* max,
-                    units::microsecond_t* deadbandMax,
-                    units::microsecond_t* center,
-                    units::microsecond_t* deadbandMin,
-                    units::microsecond_t* min) {
-  int32_t status = 0;
-  int32_t rawMax, rawDeadbandMax, rawCenter, rawDeadbandMin, rawMin;
-  HAL_GetPWMConfigMicroseconds(m_handle, &rawMax, &rawDeadbandMax, &rawCenter,
-                               &rawDeadbandMin, &rawMin, &status);
-  *max = units::microsecond_t{static_cast<double>(rawMax)};
-  *deadbandMax = units::microsecond_t{static_cast<double>(rawDeadbandMax)};
-  *center = units::microsecond_t{static_cast<double>(rawCenter)};
-  *deadbandMin = units::microsecond_t{static_cast<double>(rawDeadbandMin)};
-  *min = units::microsecond_t{static_cast<double>(rawMin)};
-  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
-}
-
-void PWM::SetAlwaysHighMode() {
-  int32_t status = 0;
-  HAL_SetPWMAlwaysHighMode(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
-}
-
 int PWM::GetChannel() const {
   return m_channel;
 }
 
+void PWM::SetSimDevice(HAL_SimDeviceHandle device) {
+  HAL_SetPWMSimDevice(m_handle, device);
+}
+
 void PWM::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("PWM");
   builder.SetActuator(true);
   builder.AddDoubleProperty(
       "Value", [=, this] { return GetPulseTime().value(); },
       [=, this](double value) { SetPulseTime(units::millisecond_t{value}); });
-  builder.AddDoubleProperty(
-      "Speed", [=, this] { return GetSpeed(); },
-      [=, this](double value) { SetSpeed(value); });
-  builder.AddDoubleProperty(
-      "Position", [=, this] { return GetPosition(); },
-      [=, this](double value) { SetPosition(value); });
 }

wpilibc/src/main/native/cpp/Servo.cpp

@@ -10,33 +10,44 @@
 
 using namespace frc;
 
-constexpr double Servo::kMaxServoAngle;
-constexpr double Servo::kMinServoAngle;
-
-constexpr units::millisecond_t Servo::kDefaultMaxServoPWM;
-constexpr units::millisecond_t Servo::kDefaultMinServoPWM;
-
-Servo::Servo(int channel) : PWM(channel) {
-  // Set minimum and maximum PWM values supported by the servo
-  SetBounds(kDefaultMaxServoPWM, 0.0_ms, 0.0_ms, 0.0_ms, kDefaultMinServoPWM);
+Servo::Servo(int channel) : m_pwm(channel, false) {
+  wpi::SendableRegistry::Add(this, "Servo", channel);
 
   // Assign defaults for period multiplier for the servo PWM control signal
-  SetPeriodMultiplier(kPeriodMultiplier_4X);
+  m_pwm.SetOutputPeriod(PWM::kOutputPeriod_20Ms);
 
   HAL_ReportUsage("IO", channel, "Servo");
   wpi::SendableRegistry::SetName(this, "Servo", channel);
+
+  m_simDevice = hal::SimDevice{"Servo", channel};
+  if (m_simDevice) {
+    m_simPosition = m_simDevice.CreateDouble("Position", true, 0.0);
+    m_pwm.SetSimDevice(m_simDevice);
+  }
 }
 
 void Servo::Set(double value) {
-  SetPosition(value);
-}
+  value = std::clamp(value, 0.0, 1.0);
+  if (m_simPosition) {
+    m_simPosition.Set(value);
+  }
 
-void Servo::SetOffline() {
-  SetDisabled();
+  units::microsecond_t rawValue =
+      (value * GetFullRangeScaleFactor()) + m_minPwm;
+
+  m_pwm.SetPulseTime(rawValue);
 }
 
 double Servo::Get() const {
-  return GetPosition();
+  units::microsecond_t rawValue = m_pwm.GetPulseTime();
+
+  if (rawValue < m_minPwm) {
+    return 0.0;
+  } else if (rawValue > m_maxPwm) {
+    return 1.0;
+  }
+  return (rawValue - m_minPwm).to<double>() /
+         GetFullRangeScaleFactor().to<double>();
 }
 
 void Servo::SetAngle(double degrees) {
@@ -46,19 +57,11 @@ void Servo::SetAngle(double degrees) {
     degrees = kMaxServoAngle;
   }
 
-  SetPosition((degrees - kMinServoAngle) / GetServoAngleRange());
+  Set((degrees - kMinServoAngle) / GetServoAngleRange());
 }
 
 double Servo::GetAngle() const {
-  return GetPosition() * GetServoAngleRange() + kMinServoAngle;
-}
-
-double Servo::GetMaxAngle() const {
-  return kMaxServoAngle;
-}
-
-double Servo::GetMinAngle() const {
-  return kMinServoAngle;
+  return Get() * GetServoAngleRange() + kMinServoAngle;
 }
 
 void Servo::InitSendable(wpi::SendableBuilder& builder) {
@@ -68,6 +71,14 @@ void Servo::InitSendable(wpi::SendableBuilder& builder) {
       [=, this](double value) { Set(value); });
 }
 
-double Servo::GetServoAngleRange() const {
+double Servo::GetServoAngleRange() {
   return kMaxServoAngle - kMinServoAngle;
 }
+
+units::microsecond_t Servo::GetFullRangeScaleFactor() const {
+  return m_maxPwm - m_minPwm;
+}
+
+int Servo::GetChannel() const {
+  return m_pwm.GetChannel();
+}

wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp

@@ -18,7 +18,7 @@ void PWMMotorController::Set(double speed) {
   if (m_isInverted) {
     speed = -speed;
   }
-  m_pwm.SetSpeed(speed);
+  SetSpeed(speed);
 
   for (auto& follower : m_nonowningFollowers) {
     follower->Set(speed);
@@ -36,7 +36,7 @@ void PWMMotorController::SetVoltage(units::volt_t output) {
 }
 
 double PWMMotorController::Get() const {
-  return m_pwm.GetSpeed() * (m_isInverted ? -1.0 : 1.0);
+  return GetSpeed() * (m_isInverted ? -1.0 : 1.0);
 }
 
 units::volt_t PWMMotorController::GetVoltage() const {
@@ -64,7 +64,11 @@ void PWMMotorController::Disable() {
 
 void PWMMotorController::StopMotor() {
   // Don't use Set(0) as that will feed the watch kitty
-  m_pwm.SetSpeed(0);
+  m_pwm.SetPulseTime(0_us);
+
+  if (m_simSpeed) {
+    m_simSpeed.Set(0.0);
+  }
 
   for (auto& follower : m_nonowningFollowers) {
     follower->StopMotor();
@@ -83,7 +87,7 @@ int PWMMotorController::GetChannel() const {
 }
 
 void PWMMotorController::EnableDeadbandElimination(bool eliminateDeadband) {
-  m_pwm.EnableDeadbandElimination(eliminateDeadband);
+  m_eliminateDeadband = eliminateDeadband;
 }
 
 void PWMMotorController::AddFollower(PWMMotorController& follower) {
@@ -95,6 +99,12 @@ WPI_IGNORE_DEPRECATED
 PWMMotorController::PWMMotorController(std::string_view name, int channel)
     : m_pwm(channel, false) {
   wpi::SendableRegistry::Add(this, name, channel);
+
+  m_simDevice = hal::SimDevice{"PWMMotorController", channel};
+  if (m_simDevice) {
+    m_simSpeed = m_simDevice.CreateDouble("Speed", true, 0.0);
+    m_pwm.SetSimDevice(m_simDevice);
+  }
 }
 
 WPI_UNIGNORE_DEPRECATED
@@ -106,3 +116,86 @@ void PWMMotorController::InitSendable(wpi::SendableBuilder& builder) {
       "Value", [=, this] { return Get(); },
       [=, this](double value) { Set(value); });
 }
+
+units::microsecond_t PWMMotorController::GetMinPositivePwm() const {
+  if (m_eliminateDeadband) {
+    return m_deadbandMaxPwm;
+  } else {
+    return m_centerPwm + 1_us;
+  }
+}
+
+units::microsecond_t PWMMotorController::GetMaxNegativePwm() const {
+  if (m_eliminateDeadband) {
+    return m_deadbandMinPwm;
+  } else {
+    return m_centerPwm - 1_us;
+  }
+}
+
+units::microsecond_t PWMMotorController::GetPositiveScaleFactor() const {
+  return m_maxPwm - GetMinPositivePwm();
+}
+
+units::microsecond_t PWMMotorController::GetNegativeScaleFactor() const {
+  return GetMaxNegativePwm() - m_minPwm;
+}
+
+void PWMMotorController::SetSpeed(double speed) {
+  if (std::isfinite(speed)) {
+    speed = std::clamp(speed, -1.0, 1.0);
+  } else {
+    speed = 0.0;
+  }
+
+  if (m_simSpeed) {
+    m_simSpeed.Set(speed);
+  }
+
+  units::microsecond_t rawValue;
+  if (speed == 0.0) {
+    rawValue = m_centerPwm;
+  } else if (speed > 0.0) {
+    rawValue = units::microsecond_t{static_cast<double>(std::lround(
+                   (speed * GetPositiveScaleFactor()).to<double>()))} +
+               GetMinPositivePwm();
+  } else {
+    rawValue = units::microsecond_t{static_cast<double>(std::lround(
+                   (speed * GetNegativeScaleFactor()).to<double>()))} +
+               GetMaxNegativePwm();
+  }
+
+  m_pwm.SetPulseTime(rawValue);
+}
+
+double PWMMotorController::GetSpeed() const {
+  units::microsecond_t rawValue = m_pwm.GetPulseTime();
+
+  if (rawValue == 0_us) {
+    return 0.0;
+  } else if (rawValue > m_maxPwm) {
+    return 1.0;
+  } else if (rawValue < m_minPwm) {
+    return -1.0;
+  } else if (rawValue > GetMinPositivePwm()) {
+    return ((rawValue - GetMinPositivePwm()) / GetPositiveScaleFactor())
+        .to<double>();
+  } else if (rawValue < GetMaxNegativePwm()) {
+    return ((rawValue - GetMaxNegativePwm()) / GetNegativeScaleFactor())
+        .to<double>();
+  } else {
+    return 0.0;
+  }
+}
+
+void PWMMotorController::SetBounds(units::microsecond_t maxPwm,
+                                   units::microsecond_t deadbandMaxPwm,
+                                   units::microsecond_t centerPwm,
+                                   units::microsecond_t deadbandMinPwm,
+                                   units::microsecond_t minPwm) {
+  m_maxPwm = maxPwm;
+  m_deadbandMaxPwm = deadbandMaxPwm;
+  m_centerPwm = centerPwm;
+  m_deadbandMinPwm = deadbandMinPwm;
+  m_minPwm = minPwm;
+}

wpilibc/src/main/native/cpp/simulation/PWMMotorControllerSim.cpp

@@ -0,0 +1,26 @@
+// 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/simulation/PWMMotorControllerSim.h"
+
+#include <hal/SimDevice.h>
+#include <units/length.h>
+
+#include "frc/simulation/SimDeviceSim.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+PWMMotorControllerSim::PWMMotorControllerSim(
+    const PWMMotorController& motorctrl)
+    : PWMMotorControllerSim(motorctrl.GetChannel()) {}
+
+PWMMotorControllerSim::PWMMotorControllerSim(int channel) {
+  frc::sim::SimDeviceSim deviceSim{"PWMMotorController", channel};
+  m_simSpeed = deviceSim.GetDouble("Speed");
+}
+
+double PWMMotorControllerSim::GetSpeed() const {
+  return m_simSpeed.Get();
+}

wpilibc/src/main/native/cpp/simulation/PWMSim.cpp

@@ -16,9 +16,6 @@ using namespace frc::sim;
 
 PWMSim::PWMSim(const PWM& pwm) : m_index{pwm.GetChannel()} {}
 
-PWMSim::PWMSim(const PWMMotorController& motorctrl)
-    : m_index{motorctrl.GetChannel()} {}
-
 PWMSim::PWMSim(int channel) : m_index{channel} {}
 
 std::unique_ptr<CallbackStore> PWMSim::RegisterInitializedCallback(
@@ -55,72 +52,21 @@ void PWMSim::SetPulseMicrosecond(int32_t microsecondPulseTime) {
   HALSIM_SetPWMPulseMicrosecond(m_index, microsecondPulseTime);
 }
 
-std::unique_ptr<CallbackStore> PWMSim::RegisterSpeedCallback(
-    NotifyCallback callback, bool initialNotify) {
-  auto store = std::make_unique<CallbackStore>(m_index, -1, callback,
-                                               &HALSIM_CancelPWMSpeedCallback);
-  store->SetUid(HALSIM_RegisterPWMSpeedCallback(m_index, &CallbackStoreThunk,
-                                                store.get(), initialNotify));
-  return store;
-}
-
-double PWMSim::GetSpeed() const {
-  return HALSIM_GetPWMSpeed(m_index);
-}
-
-void PWMSim::SetSpeed(double speed) {
-  HALSIM_SetPWMSpeed(m_index, speed);
-}
-
-std::unique_ptr<CallbackStore> PWMSim::RegisterPositionCallback(
+std::unique_ptr<CallbackStore> PWMSim::RegisterOutputPeriodCallback(
     NotifyCallback callback, bool initialNotify) {
   auto store = std::make_unique<CallbackStore>(
-      m_index, -1, callback, &HALSIM_CancelPWMPositionCallback);
-  store->SetUid(HALSIM_RegisterPWMPositionCallback(m_index, &CallbackStoreThunk,
-                                                   store.get(), initialNotify));
-  return store;
-}
-
-double PWMSim::GetPosition() const {
-  return HALSIM_GetPWMPosition(m_index);
-}
-
-void PWMSim::SetPosition(double position) {
-  HALSIM_SetPWMPosition(m_index, position);
-}
-
-std::unique_ptr<CallbackStore> PWMSim::RegisterPeriodScaleCallback(
-    NotifyCallback callback, bool initialNotify) {
-  auto store = std::make_unique<CallbackStore>(
-      m_index, -1, callback, &HALSIM_CancelPWMPeriodScaleCallback);
-  store->SetUid(HALSIM_RegisterPWMPeriodScaleCallback(
+      m_index, -1, callback, &HALSIM_CancelPWMOutputPeriodCallback);
+  store->SetUid(HALSIM_RegisterPWMOutputPeriodCallback(
       m_index, &CallbackStoreThunk, store.get(), initialNotify));
   return store;
 }
 
-int PWMSim::GetPeriodScale() const {
-  return HALSIM_GetPWMPeriodScale(m_index);
+int PWMSim::GetOutputPeriod() const {
+  return HALSIM_GetPWMOutputPeriod(m_index);
 }
 
-void PWMSim::SetPeriodScale(int periodScale) {
-  HALSIM_SetPWMPeriodScale(m_index, periodScale);
-}
-
-std::unique_ptr<CallbackStore> PWMSim::RegisterZeroLatchCallback(
-    NotifyCallback callback, bool initialNotify) {
-  auto store = std::make_unique<CallbackStore>(
-      m_index, -1, callback, &HALSIM_CancelPWMZeroLatchCallback);
-  store->SetUid(HALSIM_RegisterPWMZeroLatchCallback(
-      m_index, &CallbackStoreThunk, store.get(), initialNotify));
-  return store;
-}
-
-bool PWMSim::GetZeroLatch() const {
-  return HALSIM_GetPWMZeroLatch(m_index);
-}
-
-void PWMSim::SetZeroLatch(bool zeroLatch) {
-  HALSIM_SetPWMZeroLatch(m_index, zeroLatch);
+void PWMSim::SetOutputPeriod(int period) {
+  HALSIM_SetPWMOutputPeriod(m_index, period);
 }
 
 void PWMSim::ResetData() {

wpilibc/src/main/native/cpp/simulation/ServoSim.cpp

@@ -0,0 +1,28 @@
+// 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/simulation/ServoSim.h"
+
+#include <hal/SimDevice.h>
+#include <units/length.h>
+
+#include "frc/simulation/SimDeviceSim.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+ServoSim::ServoSim(const Servo& servo) : ServoSim(servo.GetChannel()) {}
+
+ServoSim::ServoSim(int channel) {
+  frc::sim::SimDeviceSim deviceSim{"Servo", channel};
+  m_simPosition = deviceSim.GetDouble("Position");
+}
+
+double ServoSim::GetPosition() const {
+  return m_simPosition.Get();
+}
+
+double ServoSim::GetAngle() const {
+  return GetPosition() * Servo::GetServoAngleRange() + Servo::kMinServoAngle;
+}

wpilibc/src/main/native/include/frc/PWM.h

@@ -27,21 +27,21 @@ class PWM : public wpi::Sendable, public wpi::SendableHelper<PWM> {
  public:
   friend class AddressableLED;
   /**
-   * Represents the amount to multiply the minimum servo-pulse pwm period by.
+   * Represents the output period in microseconds.
    */
-  enum PeriodMultiplier {
+  enum OutputPeriod {
     /**
-     * Don't skip pulses. PWM pulses occur every 5.05 ms
+     * PWM pulses occur every 5 ms
      */
-    kPeriodMultiplier_1X = 1,
+    kOutputPeriod_5Ms = 1,
     /**
-     * Skip every other pulse. PWM pulses occur every 10.10 ms
+     * PWM pulses occur every 10 ms
      */
-    kPeriodMultiplier_2X = 2,
+    kOutputPeriod_10Ms = 2,
     /**
-     * Skip three out of four pulses. PWM pulses occur every 20.20 ms
+     * PWM pulses occur every 20 ms
      */
-    kPeriodMultiplier_4X = 4
+    kOutputPeriod_20Ms = 4
   };
 
   /**
@@ -74,7 +74,7 @@ class PWM : public wpi::Sendable, public wpi::SendableHelper<PWM> {
    *
    * @param time Microsecond PWM value.
    */
-  virtual void SetPulseTime(units::microsecond_t time);
+  void SetPulseTime(units::microsecond_t time);
 
   /**
    * Get the PWM pulse time directly from the hardware.
@@ -83,122 +83,30 @@ class PWM : public wpi::Sendable, public wpi::SendableHelper<PWM> {
    *
    * @return Microsecond PWM control value.
    */
-  virtual units::microsecond_t GetPulseTime() const;
-
-  /**
-   * Set the PWM value based on a position.
-   *
-   * This is intended to be used by servos.
-   *
-   * @pre SetBounds() called.
-   *
-   * @param pos The position to set the servo between 0.0 and 1.0.
-   */
-  virtual void SetPosition(double pos);
-
-  /**
-   * Get the PWM value in terms of a position.
-   *
-   * This is intended to be used by servos.
-   *
-   * @pre SetBounds() called.
-   *
-   * @return The position the servo is set to between 0.0 and 1.0.
-   */
-  virtual double GetPosition() const;
-
-  /**
-   * Set the PWM value based on a speed.
-   *
-   * This is intended to be used by motor controllers.
-   *
-   * @pre SetBounds() called.
-   *
-   * @param speed The speed to set the motor controller between -1.0 and 1.0.
-   */
-  virtual void SetSpeed(double speed);
-
-  /**
-   * Get the PWM value in terms of speed.
-   *
-   * This is intended to be used by motor controllers.
-   *
-   * @pre SetBounds() called.
-   *
-   * @return The most recently set speed between -1.0 and 1.0.
-   */
-  virtual double GetSpeed() const;
+  units::microsecond_t GetPulseTime() const;
 
   /**
    * Temporarily disables the PWM output. The next set call will re-enable
    * the output.
    */
-  virtual void SetDisabled();
+  void SetDisabled();
 
   /**
-   * Slow down the PWM signal for old devices.
+   * Sets the PWM output period.
    *
-   * @param mult The period multiplier to apply to this channel
+   * @param mult The output period to apply to this channel
    */
-  void SetPeriodMultiplier(PeriodMultiplier mult);
-
-  /**
-   * Latches PWM to zero.
-   */
-  void SetZeroLatch();
-
-  /**
-   * Optionally eliminate the deadband from a motor controller.
-   *
-   * @param eliminateDeadband If true, set the motor curve on the motor
-   *                          controller to eliminate the deadband in the middle
-   *                          of the range. Otherwise, keep the full range
-   *                          without modifying any values.
-   */
-  void EnableDeadbandElimination(bool eliminateDeadband);
-
-  /**
-   * Set the bounds on the PWM pulse widths.
-   *
-   * This sets the bounds on the PWM values for a particular type of controller.
-   * The values determine the upper and lower speeds as well as the deadband
-   * bracket.
-   *
-   * @param max         The max PWM pulse width in us
-   * @param deadbandMax The high end of the deadband range pulse width in us
-   * @param center      The center (off) pulse width in us
-   * @param deadbandMin The low end of the deadband pulse width in us
-   * @param min         The minimum pulse width in us
-   */
-  void SetBounds(units::microsecond_t max, units::microsecond_t deadbandMax,
-                 units::microsecond_t center, units::microsecond_t deadbandMin,
-                 units::microsecond_t min);
-
-  /**
-   * Get the bounds on the PWM values.
-   *
-   * This gets the bounds on the PWM values for a particular each type of
-   * controller. The values determine the upper and lower speeds as well as the
-   * deadband bracket.
-   *
-   * @param max         The maximum pwm value
-   * @param deadbandMax The high end of the deadband range
-   * @param center      The center speed (off)
-   * @param deadbandMin The low end of the deadband range
-   * @param min         The minimum pwm value
-   */
-  void GetBounds(units::microsecond_t* max, units::microsecond_t* deadbandMax,
-                 units::microsecond_t* center,
-                 units::microsecond_t* deadbandMin, units::microsecond_t* min);
-
-  /**
-   * Sets the PWM output to be a continuous high signal while enabled.
-   *
-   */
-  void SetAlwaysHighMode();
+  void SetOutputPeriod(OutputPeriod mult);
 
   int GetChannel() const;
 
+  /**
+   * Indicates this input is used by a simulated device.
+   *
+   * @param device simulated device handle
+   */
+  void SetSimDevice(HAL_SimDeviceHandle device);
+
  protected:
   void InitSendable(wpi::SendableBuilder& builder) override;
 

wpilibc/src/main/native/include/frc/Servo.h

@@ -4,20 +4,27 @@
 
 #pragma once
 
+#include <hal/SimDevice.h>
 #include <units/angle.h>
 
 #include "frc/PWM.h"
 
 namespace frc {
 
+namespace sim {
+class ServoSim;
+}  // namespace sim
+
 /**
  * Standard hobby style servo.
  *
  * The range parameters default to the appropriate values for the Hitec HS-322HD
  * servo provided in the FIRST Kit of Parts in 2008.
  */
-class Servo : public PWM {
+class Servo : public wpi::Sendable, public wpi::SendableHelper<Servo> {
  public:
+  friend class frc::sim::ServoSim;
+
   /**
    * Constructor.
    *
@@ -42,13 +49,6 @@ class Servo : public PWM {
    */
   void Set(double value);
 
-  /**
-   * Set the servo to offline.
-   *
-   * Set the servo raw value to 0 (undriven)
-   */
-  void SetOffline();
-
   /**
    * Get the servo position.
    *
@@ -86,30 +86,27 @@ class Servo : public PWM {
    */
   double GetAngle() const;
 
-  /**
-   * Get the maximum angle of the servo.
-   *
-   * @return The maximum angle of the servo in degrees.
-   */
-  double GetMaxAngle() const;
-
-  /**
-   * Get the minimum angle of the servo.
-   *
-   * @return The minimum angle of the servo in degrees.
-   */
-  double GetMinAngle() const;
+  int GetChannel() const;
 
   void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
-  double GetServoAngleRange() const;
+  static double GetServoAngleRange();
+  units::microsecond_t GetFullRangeScaleFactor() const;
 
-  static constexpr double kMaxServoAngle = 180.;
+  static constexpr double kMaxServoAngle = 180.0;
   static constexpr double kMinServoAngle = 0.0;
 
   static constexpr units::millisecond_t kDefaultMaxServoPWM = 2.4_ms;
   static constexpr units::millisecond_t kDefaultMinServoPWM = 0.6_ms;
+
+  units::millisecond_t m_maxPwm = kDefaultMaxServoPWM;
+  units::millisecond_t m_minPwm = kDefaultMinServoPWM;
+
+  hal::SimDevice m_simDevice;
+  hal::SimDouble m_simPosition;
+
+  PWM m_pwm;
 };
 
 }  // namespace frc

wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h

@@ -12,6 +12,7 @@
 #include <utility>
 #include <vector>
 
+#include <hal/SimDevice.h>
 #include <units/voltage.h>
 #include <wpi/deprecated.h>
 #include <wpi/sendable/Sendable.h>
@@ -133,11 +134,35 @@ class PWMMotorController : public MotorController,
   /// PWM instances for motor controller.
   PWM m_pwm;
 
+  void SetSpeed(double speed);
+  double GetSpeed() const;
+
+  void SetBounds(units::microsecond_t maxPwm,
+                 units::microsecond_t deadbandMaxPwm,
+                 units::microsecond_t centerPwm,
+                 units::microsecond_t deadbandMinPwm,
+                 units::microsecond_t minPwm);
+
  private:
   bool m_isInverted = false;
   std::vector<PWMMotorController*> m_nonowningFollowers;
   std::vector<std::unique_ptr<PWMMotorController>> m_owningFollowers;
 
+  hal::SimDevice m_simDevice;
+  hal::SimDouble m_simSpeed;
+
+  bool m_eliminateDeadband{0};
+  units::microsecond_t m_minPwm{0};
+  units::microsecond_t m_deadbandMinPwm{0};
+  units::microsecond_t m_centerPwm{0};
+  units::microsecond_t m_deadbandMaxPwm{0};
+  units::microsecond_t m_maxPwm{0};
+
+  units::microsecond_t GetMinPositivePwm() const;
+  units::microsecond_t GetMaxNegativePwm() const;
+  units::microsecond_t GetPositiveScaleFactor() const;
+  units::microsecond_t GetNegativeScaleFactor() const;
+
   PWM* GetPwm() { return &m_pwm; }
 };
 

wpilibc/src/main/native/include/frc/simulation/PWMMotorControllerSim.h

@@ -0,0 +1,30 @@
+// 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 <hal/SimDevice.h>
+#include <units/length.h>
+
+#include "frc/motorcontrol/PWMMotorController.h"
+
+namespace frc {
+
+class PWMMotorController;
+
+namespace sim {
+
+class PWMMotorControllerSim {
+ public:
+  explicit PWMMotorControllerSim(const PWMMotorController& motorctrl);
+
+  explicit PWMMotorControllerSim(int channel);
+
+  double GetSpeed() const;
+
+ private:
+  hal::SimDouble m_simSpeed;
+};
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/frc/simulation/PWMSim.h

@@ -27,13 +27,6 @@ class PWMSim {
    */
   explicit PWMSim(const PWM& pwm);
 
-  /**
-   * Constructs from a PWMMotorController object.
-   *
-   * @param motorctrl PWMMotorController to simulate
-   */
-  explicit PWMSim(const PWMMotorController& motorctrl);
-
   /**
    * Constructs from a PWM channel number.
    *
@@ -91,56 +84,6 @@ class PWMSim {
    */
   void SetPulseMicrosecond(int32_t microsecondPulseTime);
 
-  /**
-   * Register a callback to be run when the PWM speed changes.
-   *
-   * @param callback the callback
-   * @param initialNotify whether to run the callback with the initial value
-   * @return the CallbackStore object associated with this callback
-   */
-  [[nodiscard]]
-  std::unique_ptr<CallbackStore> RegisterSpeedCallback(NotifyCallback callback,
-                                                       bool initialNotify);
-
-  /**
-   * Get the PWM speed.
-   *
-   * @return the PWM speed (-1.0 to 1.0)
-   */
-  double GetSpeed() const;
-
-  /**
-   * Set the PWM speed.
-   *
-   * @param speed the PWM speed (-1.0 to 1.0)
-   */
-  void SetSpeed(double speed);
-
-  /**
-   * Register a callback to be run when the PWM position changes.
-   *
-   * @param callback the callback
-   * @param initialNotify whether to run the callback with the initial value
-   * @return the CallbackStore object associated with this callback
-   */
-  [[nodiscard]]
-  std::unique_ptr<CallbackStore> RegisterPositionCallback(
-      NotifyCallback callback, bool initialNotify);
-
-  /**
-   * Get the PWM position.
-   *
-   * @return the PWM position (0.0 to 1.0)
-   */
-  double GetPosition() const;
-
-  /**
-   * Set the PWM position.
-   *
-   * @param position the PWM position (0.0 to 1.0)
-   */
-  void SetPosition(double position);
-
   /**
    * Register a callback to be run when the PWM period scale changes.
    *
@@ -149,7 +92,7 @@ class PWMSim {
    * @return the CallbackStore object associated with this callback
    */
   [[nodiscard]]
-  std::unique_ptr<CallbackStore> RegisterPeriodScaleCallback(
+  std::unique_ptr<CallbackStore> RegisterOutputPeriodCallback(
       NotifyCallback callback, bool initialNotify);
 
   /**
@@ -157,39 +100,14 @@ class PWMSim {
    *
    * @return the PWM period scale
    */
-  int GetPeriodScale() const;
+  int GetOutputPeriod() const;
 
   /**
    * Set the PWM period scale.
    *
-   * @param periodScale the PWM period scale
+   * @param period the PWM period scale
    */
-  void SetPeriodScale(int periodScale);
-
-  /**
-   * Register a callback to be run when the PWM zero latch state changes.
-   *
-   * @param callback the callback
-   * @param initialNotify whether to run the callback with the initial state
-   * @return the CallbackStore object associated with this callback
-   */
-  [[nodiscard]]
-  std::unique_ptr<CallbackStore> RegisterZeroLatchCallback(
-      NotifyCallback callback, bool initialNotify);
-
-  /**
-   * Check whether the PWM is zero latched.
-   *
-   * @return true if zero latched
-   */
-  bool GetZeroLatch() const;
-
-  /**
-   * Define whether the PWM has been zero latched.
-   *
-   * @param zeroLatch true to indicate zero latched
-   */
-  void SetZeroLatch(bool zeroLatch);
+  void SetOutputPeriod(int period);
 
   /**
    * Reset all simulation data.

wpilibc/src/main/native/include/frc/simulation/ServoSim.h

@@ -0,0 +1,31 @@
+// 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 <hal/SimDevice.h>
+#include <units/length.h>
+
+#include "frc/Servo.h"
+
+namespace frc {
+
+class Servo;
+
+namespace sim {
+class ServoSim {
+ public:
+  explicit ServoSim(const Servo& servo);
+
+  explicit ServoSim(int channel);
+
+  double GetPosition() const;
+
+  double GetAngle() const;
+
+ private:
+  hal::SimDouble m_simPosition;
+};
+}  // namespace sim
+}  // namespace frc