[hal] Unify PWM simulation Speed, Position, and Raw (#5277)

Setting one will set the others, like it does in real hardware.
Add tests for boundary conditions and conversions.
Update PWM sendable implementation to include all forms.
Fixes #5264
Fixes #3606

hal/src/main/native/sim/PWM.cpp

@@ -4,6 +4,9 @@
 
 #include "hal/PWM.h"
 
+#include <algorithm>
+#include <cmath>
+
 #include "ConstantsInternal.h"
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
@@ -18,6 +21,46 @@ namespace hal::init {
 void InitializePWM() {}
 }  // namespace hal::init
 
+static inline int32_t GetMaxPositivePwm(DigitalPort* port) {
+  return port->maxPwm;
+}
+
+static inline int32_t GetMinPositivePwm(DigitalPort* port) {
+  if (port->eliminateDeadband) {
+    return port->deadbandMaxPwm;
+  } else {
+    return port->centerPwm + 1;
+  }
+}
+
+static inline int32_t GetCenterPwm(DigitalPort* port) {
+  return port->centerPwm;
+}
+
+static inline int32_t GetMaxNegativePwm(DigitalPort* port) {
+  if (port->eliminateDeadband) {
+    return port->deadbandMinPwm;
+  } else {
+    return port->centerPwm - 1;
+  }
+}
+
+static inline int32_t GetMinNegativePwm(DigitalPort* port) {
+  return port->minPwm;
+}
+
+static inline int32_t GetPositiveScaleFactor(DigitalPort* port) {
+  return GetMaxPositivePwm(port) - GetMinPositivePwm(port);
+}  ///< The scale for positive speeds.
+
+static inline int32_t GetNegativeScaleFactor(DigitalPort* port) {
+  return GetMaxNegativePwm(port) - GetMinNegativePwm(port);
+}  ///< The scale for negative speeds.
+
+static inline int32_t GetFullRangeScaleFactor(DigitalPort* port) {
+  return GetMaxPositivePwm(port) - GetMinNegativePwm(port);
+}  ///< The scale for positions.
+
 extern "C" {
 
 HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle,
@@ -147,6 +190,40 @@ void HAL_SetPWMPulseTimeMicroseconds(HAL_DigitalHandle pwmPortHandle,
   }
 
   SimPWMData[port->channel].pulseMicrosecond = value;
+
+  DigitalPort* dPort = port.get();
+  double speed = 0.0;
+
+  if (value == kPwmDisabled) {
+    speed = 0.0;
+  } else if (value > GetMaxPositivePwm(dPort)) {
+    speed = 1.0;
+  } else if (value < GetMinNegativePwm(dPort)) {
+    speed = -1.0;
+  } else if (value > GetMinPositivePwm(dPort)) {
+    speed = static_cast<double>(value - GetMinPositivePwm(dPort)) /
+            static_cast<double>(GetPositiveScaleFactor(dPort));
+  } else if (value < GetMaxNegativePwm(dPort)) {
+    speed = static_cast<double>(value - GetMaxNegativePwm(dPort)) /
+            static_cast<double>(GetNegativeScaleFactor(dPort));
+  } else {
+    speed = 0.0;
+  }
+
+  SimPWMData[port->channel].speed = speed;
+
+  double pos = 0.0;
+
+  if (value < GetMinNegativePwm(dPort)) {
+    pos = 0.0;
+  } else if (value > GetMaxPositivePwm(dPort)) {
+    pos = 1.0;
+  } else {
+    pos = static_cast<double>(value - GetMinNegativePwm(dPort)) /
+          static_cast<double>(GetFullRangeScaleFactor(dPort));
+  }
+
+  SimPWMData[port->channel].position = pos;
 }
 
 void HAL_SetPWMSpeed(HAL_DigitalHandle pwmPortHandle, double speed,
@@ -161,13 +238,36 @@ void HAL_SetPWMSpeed(HAL_DigitalHandle pwmPortHandle, double speed,
     return;
   }
 
-  if (speed < -1.0) {
-    speed = -1.0;
-  } else if (speed > 1.0) {
-    speed = 1.0;
+  if (std::isfinite(speed)) {
+    speed = std::clamp(speed, -1.0, 1.0);
+  } else {
+    speed = 0.0;
   }
 
-  SimPWMData[port->channel].speed = speed;
+  DigitalPort* dPort = port.get();
+
+  // calculate the desired output pwm value by scaling the speed appropriately
+  int32_t rawValue;
+  if (speed == 0.0) {
+    rawValue = GetCenterPwm(dPort);
+  } else if (speed > 0.0) {
+    rawValue =
+        std::lround(speed * static_cast<double>(GetPositiveScaleFactor(dPort)) +
+                    static_cast<double>(GetMinPositivePwm(dPort)));
+  } else {
+    rawValue =
+        std::lround(speed * static_cast<double>(GetNegativeScaleFactor(dPort)) +
+                    static_cast<double>(GetMaxNegativePwm(dPort)));
+  }
+
+  if (!((rawValue >= GetMinNegativePwm(dPort)) &&
+        (rawValue <= GetMaxPositivePwm(dPort))) ||
+      rawValue == kPwmDisabled) {
+    *status = HAL_PWM_SCALE_ERROR;
+    return;
+  }
+
+  HAL_SetPWMPulseTimeMicroseconds(pwmPortHandle, rawValue, status);
 }
 
 void HAL_SetPWMPosition(HAL_DigitalHandle pwmPortHandle, double pos,
@@ -188,7 +288,20 @@ void HAL_SetPWMPosition(HAL_DigitalHandle pwmPortHandle, double pos,
     pos = 1.0;
   }
 
-  SimPWMData[port->channel].position = pos;
+  DigitalPort* dPort = port.get();
+
+  // note, need to perform the multiplication below as floating point before
+  // converting to int
+  int32_t rawValue = static_cast<int32_t>(
+      (pos * static_cast<double>(GetFullRangeScaleFactor(dPort))) +
+      GetMinNegativePwm(dPort));
+
+  if (rawValue == kPwmDisabled) {
+    *status = HAL_PWM_SCALE_ERROR;
+    return;
+  }
+
+  HAL_SetPWMPulseTimeMicroseconds(pwmPortHandle, rawValue, status);
 }
 
 void HAL_SetPWMDisabled(HAL_DigitalHandle pwmPortHandle, int32_t* status) {

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

@@ -180,4 +180,10 @@ void PWM::InitSendable(wpi::SendableBuilder& builder) {
   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/test/native/cpp/simulation/PWMSimTest.cpp

@@ -12,6 +12,8 @@
 
 namespace frc::sim {
 
+constexpr double kPWMStepSize = 1.0 / 2000.0;
+
 TEST(PWMSimTest, Initialize) {
   HAL_Initialize(500, 0);
 
@@ -55,13 +57,47 @@ TEST(PWMSimTest, SetSpeed) {
 
   auto cb = sim.RegisterSpeedCallback(callback.GetCallback(), false);
   PWM pwm{0};
-  constexpr double kTestValue = 0.3504;
+  double kTestValue = 0.3504;
   pwm.SetSpeed(kTestValue);
 
-  EXPECT_EQ(kTestValue, sim.GetSpeed());
-  EXPECT_EQ(kTestValue, pwm.GetSpeed());
+  EXPECT_NEAR(kTestValue, sim.GetSpeed(), kPWMStepSize);
+  EXPECT_NEAR(kTestValue, pwm.GetSpeed(), kPWMStepSize);
   EXPECT_TRUE(callback.WasTriggered());
-  EXPECT_EQ(kTestValue, callback.GetLastValue());
+  EXPECT_NEAR(kTestValue, callback.GetLastValue(), kPWMStepSize);
+  EXPECT_NEAR(kTestValue / 2 + 0.5, sim.GetPosition(), kPWMStepSize * 2);
+  EXPECT_NEAR(kTestValue / 2 + 0.5, pwm.GetPosition(), kPWMStepSize * 2);
+
+  kTestValue = -1.0;
+  pwm.SetSpeed(kTestValue);
+
+  EXPECT_NEAR(kTestValue, sim.GetSpeed(), kPWMStepSize);
+  EXPECT_NEAR(kTestValue, pwm.GetSpeed(), kPWMStepSize);
+  EXPECT_NEAR(0.0, sim.GetPosition(), kPWMStepSize);
+  EXPECT_NEAR(0.0, pwm.GetPosition(), kPWMStepSize);
+
+  kTestValue = 0.0;
+  pwm.SetSpeed(kTestValue);
+
+  EXPECT_NEAR(kTestValue, sim.GetSpeed(), kPWMStepSize);
+  EXPECT_NEAR(kTestValue, pwm.GetSpeed(), kPWMStepSize);
+  EXPECT_NEAR(0.5, sim.GetPosition(), kPWMStepSize);
+  EXPECT_NEAR(0.5, pwm.GetPosition(), kPWMStepSize);
+
+  kTestValue = 1.0;
+  pwm.SetSpeed(kTestValue);
+
+  EXPECT_NEAR(kTestValue, sim.GetSpeed(), kPWMStepSize);
+  EXPECT_NEAR(kTestValue, pwm.GetSpeed(), kPWMStepSize);
+  EXPECT_NEAR(kTestValue, sim.GetPosition(), kPWMStepSize);
+  EXPECT_NEAR(kTestValue, pwm.GetPosition(), kPWMStepSize);
+
+  kTestValue = 1.1;
+  pwm.SetSpeed(kTestValue);
+
+  EXPECT_NEAR(1.0, sim.GetSpeed(), kPWMStepSize);
+  EXPECT_NEAR(1.0, pwm.GetSpeed(), kPWMStepSize);
+  EXPECT_NEAR(1.0, sim.GetPosition(), kPWMStepSize);
+  EXPECT_NEAR(1.0, pwm.GetPosition(), kPWMStepSize);
 }
 
 TEST(PWMSimTest, SetPosition) {
@@ -75,13 +111,47 @@ TEST(PWMSimTest, SetPosition) {
 
   auto cb = sim.RegisterPositionCallback(callback.GetCallback(), false);
   PWM pwm{0};
-  constexpr double kTestValue = 0.3504;
+  double kTestValue = 0.3504;
   pwm.SetPosition(kTestValue);
 
-  EXPECT_EQ(kTestValue, sim.GetPosition());
-  EXPECT_EQ(kTestValue, pwm.GetPosition());
+  EXPECT_NEAR(kTestValue, sim.GetPosition(), kPWMStepSize);
+  EXPECT_NEAR(kTestValue, pwm.GetPosition(), kPWMStepSize);
   EXPECT_TRUE(callback.WasTriggered());
-  EXPECT_EQ(kTestValue, callback.GetLastValue());
+  EXPECT_NEAR(kTestValue, callback.GetLastValue(), kPWMStepSize);
+  EXPECT_NEAR(kTestValue * 2 - 1.0, sim.GetSpeed(), kPWMStepSize * 2);
+  EXPECT_NEAR(kTestValue * 2 - 1.0, pwm.GetSpeed(), kPWMStepSize * 2);
+
+  kTestValue = -1.0;
+  pwm.SetPosition(kTestValue);
+
+  EXPECT_NEAR(0.0, sim.GetPosition(), kPWMStepSize);
+  EXPECT_NEAR(0.0, pwm.GetPosition(), kPWMStepSize);
+  EXPECT_NEAR(kTestValue, sim.GetSpeed(), kPWMStepSize);
+  EXPECT_NEAR(kTestValue, pwm.GetSpeed(), kPWMStepSize);
+
+  kTestValue = 0.0;
+  pwm.SetPosition(kTestValue);
+
+  EXPECT_NEAR(kTestValue, sim.GetPosition(), kPWMStepSize);
+  EXPECT_NEAR(kTestValue, pwm.GetPosition(), kPWMStepSize);
+  EXPECT_NEAR(-1.0, sim.GetSpeed(), kPWMStepSize);
+  EXPECT_NEAR(-1.0, pwm.GetSpeed(), kPWMStepSize);
+
+  kTestValue = 1.0;
+  pwm.SetPosition(kTestValue);
+
+  EXPECT_NEAR(kTestValue, sim.GetPosition(), kPWMStepSize);
+  EXPECT_NEAR(kTestValue, pwm.GetPosition(), kPWMStepSize);
+  EXPECT_NEAR(kTestValue, sim.GetSpeed(), kPWMStepSize);
+  EXPECT_NEAR(kTestValue, pwm.GetSpeed(), kPWMStepSize);
+
+  kTestValue = 1.1;
+  pwm.SetPosition(kTestValue);
+
+  EXPECT_NEAR(1.0, sim.GetPosition(), kPWMStepSize);
+  EXPECT_NEAR(1.0, pwm.GetPosition(), kPWMStepSize);
+  EXPECT_NEAR(1.0, sim.GetSpeed(), kPWMStepSize);
+  EXPECT_NEAR(1.0, pwm.GetSpeed(), kPWMStepSize);
 }
 
 TEST(PWMSimTest, SetPeriodScale) {

wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java

@@ -253,5 +253,7 @@ public class PWM implements Sendable, AutoCloseable {
     builder.setSafeState(this::setDisabled);
     builder.addDoubleProperty(
         "Value", this::getPulseTimeMicroseconds, value -> setPulseTimeMicroseconds((int) value));
+    builder.addDoubleProperty("Speed", this::getSpeed, this::setSpeed);
+    builder.addDoubleProperty("Position", this::getPosition, this::setPosition);
   }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/PWMSimTest.java

@@ -16,6 +16,8 @@ import edu.wpi.first.wpilibj.simulation.testutils.IntCallback;
 import org.junit.jupiter.api.Test;
 
 class PWMSimTest {
+  private static final double PWM_STEP_SIZE = 1.0 / 2000.0;
+
   @Test
   void testInitialize() {
     HAL.initialize(500, 0);
@@ -64,13 +66,47 @@ class PWMSimTest {
 
     try (CallbackStore cb = sim.registerSpeedCallback(callback, false);
         PWM pwm = new PWM(0)) {
-      final double kTestValue = 0.3504;
+      double kTestValue = 0.3504;
       pwm.setSpeed(kTestValue);
 
-      assertEquals(kTestValue, sim.getSpeed());
-      assertEquals(kTestValue, pwm.getSpeed());
+      assertEquals(kTestValue, sim.getSpeed(), PWM_STEP_SIZE);
+      assertEquals(kTestValue, pwm.getSpeed(), PWM_STEP_SIZE);
       assertTrue(callback.wasTriggered());
-      assertEquals(kTestValue, callback.getSetValue());
+      assertEquals(kTestValue, callback.getSetValue(), PWM_STEP_SIZE);
+      assertEquals(kTestValue / 2 + 0.5, sim.getPosition(), PWM_STEP_SIZE * 2);
+      assertEquals(kTestValue / 2 + 0.5, pwm.getPosition(), PWM_STEP_SIZE * 2);
+
+      kTestValue = -1.0;
+      pwm.setSpeed(kTestValue);
+
+      assertEquals(kTestValue, sim.getSpeed(), PWM_STEP_SIZE);
+      assertEquals(kTestValue, pwm.getSpeed(), PWM_STEP_SIZE);
+      assertEquals(0.0, sim.getPosition(), PWM_STEP_SIZE);
+      assertEquals(0.0, pwm.getPosition(), PWM_STEP_SIZE);
+
+      kTestValue = 0.0;
+      pwm.setSpeed(kTestValue);
+
+      assertEquals(kTestValue, sim.getSpeed(), PWM_STEP_SIZE);
+      assertEquals(kTestValue, pwm.getSpeed(), PWM_STEP_SIZE);
+      assertEquals(0.5, sim.getPosition(), PWM_STEP_SIZE);
+      assertEquals(0.5, pwm.getPosition(), PWM_STEP_SIZE);
+
+      kTestValue = 1.0;
+      pwm.setSpeed(kTestValue);
+
+      assertEquals(kTestValue, sim.getSpeed(), PWM_STEP_SIZE);
+      assertEquals(kTestValue, pwm.getSpeed(), PWM_STEP_SIZE);
+      assertEquals(kTestValue, sim.getPosition(), PWM_STEP_SIZE);
+      assertEquals(kTestValue, pwm.getPosition(), PWM_STEP_SIZE);
+
+      kTestValue = 1.1;
+      pwm.setSpeed(kTestValue);
+
+      assertEquals(1.0, sim.getSpeed(), PWM_STEP_SIZE);
+      assertEquals(1.0, pwm.getSpeed(), PWM_STEP_SIZE);
+      assertEquals(1.0, sim.getPosition(), PWM_STEP_SIZE);
+      assertEquals(1.0, pwm.getPosition(), PWM_STEP_SIZE);
     }
   }
 
@@ -86,13 +122,47 @@ class PWMSimTest {
 
     try (CallbackStore cb = sim.registerPositionCallback(callback, false);
         PWM pwm = new PWM(0)) {
-      final double kTestValue = 0.3504;
+      double kTestValue = 0.3504;
       pwm.setPosition(kTestValue);
 
-      assertEquals(kTestValue, sim.getPosition());
-      assertEquals(kTestValue, pwm.getPosition());
+      assertEquals(kTestValue, sim.getPosition(), PWM_STEP_SIZE);
+      assertEquals(kTestValue, pwm.getPosition(), PWM_STEP_SIZE);
       assertTrue(callback.wasTriggered());
-      assertEquals(kTestValue, callback.getSetValue());
+      assertEquals(kTestValue, callback.getSetValue(), PWM_STEP_SIZE);
+      assertEquals(kTestValue * 2 - 1.0, sim.getSpeed(), PWM_STEP_SIZE * 2);
+      assertEquals(kTestValue * 2 - 1.0, pwm.getSpeed(), PWM_STEP_SIZE * 2);
+
+      kTestValue = -1.0;
+      pwm.setPosition(kTestValue);
+
+      assertEquals(0.0, sim.getPosition(), PWM_STEP_SIZE);
+      assertEquals(0.0, pwm.getPosition(), PWM_STEP_SIZE);
+      assertEquals(kTestValue, sim.getSpeed(), PWM_STEP_SIZE);
+      assertEquals(kTestValue, pwm.getSpeed(), PWM_STEP_SIZE);
+
+      kTestValue = 0.0;
+      pwm.setPosition(kTestValue);
+
+      assertEquals(kTestValue, sim.getPosition(), PWM_STEP_SIZE);
+      assertEquals(kTestValue, pwm.getPosition(), PWM_STEP_SIZE);
+      assertEquals(-1.0, sim.getSpeed(), PWM_STEP_SIZE);
+      assertEquals(-1.0, pwm.getSpeed(), PWM_STEP_SIZE);
+
+      kTestValue = 1.0;
+      pwm.setPosition(kTestValue);
+
+      assertEquals(kTestValue, sim.getPosition(), PWM_STEP_SIZE);
+      assertEquals(kTestValue, pwm.getPosition(), PWM_STEP_SIZE);
+      assertEquals(kTestValue, sim.getSpeed(), PWM_STEP_SIZE);
+      assertEquals(kTestValue, pwm.getSpeed(), PWM_STEP_SIZE);
+
+      kTestValue = 1.1;
+      pwm.setPosition(kTestValue);
+
+      assertEquals(1.0, sim.getPosition(), PWM_STEP_SIZE);
+      assertEquals(1.0, pwm.getPosition(), PWM_STEP_SIZE);
+      assertEquals(1.0, sim.getSpeed(), PWM_STEP_SIZE);
+      assertEquals(1.0, pwm.getSpeed(), PWM_STEP_SIZE);
     }
   }