Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
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
This commit is contained in:
sciencewhiz
2023-07-09 21:28:50 -07:00
committed by GitHub
parent fd5699b240
commit f8e74e2f7c
5 changed files with 283 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

125
hal/src/main/native/sim/PWM.cpp
View File

@@ -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) {