[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;
+}