diff --git a/hal/src/main/native/sim/PWM.cpp b/hal/src/main/native/sim/PWM.cpp index bfc38ad066..c668039b2e 100644 --- a/hal/src/main/native/sim/PWM.cpp +++ b/hal/src/main/native/sim/PWM.cpp @@ -4,6 +4,9 @@ #include "hal/PWM.h" +#include +#include + #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(value - GetMinPositivePwm(dPort)) / + static_cast(GetPositiveScaleFactor(dPort)); + } else if (value < GetMaxNegativePwm(dPort)) { + speed = static_cast(value - GetMaxNegativePwm(dPort)) / + static_cast(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(value - GetMinNegativePwm(dPort)) / + static_cast(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(GetPositiveScaleFactor(dPort)) + + static_cast(GetMinPositivePwm(dPort))); + } else { + rawValue = + std::lround(speed * static_cast(GetNegativeScaleFactor(dPort)) + + static_cast(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( + (pos * static_cast(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) { diff --git a/wpilibc/src/main/native/cpp/PWM.cpp b/wpilibc/src/main/native/cpp/PWM.cpp index df15577fa6..6a4c0f58c7 100644 --- a/wpilibc/src/main/native/cpp/PWM.cpp +++ b/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); }); } diff --git a/wpilibc/src/test/native/cpp/simulation/PWMSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/PWMSimTest.cpp index e328b1030d..71c3f2fd49 100644 --- a/wpilibc/src/test/native/cpp/simulation/PWMSimTest.cpp +++ b/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) { diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java index 482f54172c..8d6b5ba914 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java +++ b/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); } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/PWMSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/PWMSimTest.java index bff23ff705..98a05afc95 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/PWMSimTest.java +++ b/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); } }