mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-02 02:51:42 +00:00
Switches PWMs to do scaling at the HAL level. (#143)
This commit is contained in:
Thad House
committed by
Peter Johnson
Peter Johnson
parent
be2647d44e
commit
5ad28d58ec
@@ -7,12 +7,38 @@
|
||||
|
||||
#include "HAL/PWM.h"
|
||||
|
||||
#include "ConstantsInternal.h"
|
||||
#include "DigitalInternal.h"
|
||||
#include "PortsInternal.h"
|
||||
#include "handles/HandlesInternal.h"
|
||||
|
||||
using namespace hal;
|
||||
|
||||
static inline int32_t GetMaxPositivePwm(DigitalPort* port) {
|
||||
return port->maxPwm;
|
||||
}
|
||||
static inline int32_t GetMinPositivePwm(DigitalPort* port) {
|
||||
return port->eliminateDeadband ? port->deadbandMaxPwm : port->centerPwm + 1;
|
||||
}
|
||||
static inline int32_t GetCenterPwm(DigitalPort* port) {
|
||||
return port->centerPwm;
|
||||
}
|
||||
static inline int32_t GetMaxNegativePwm(DigitalPort* port) {
|
||||
return port->eliminateDeadband ? port->deadbandMinPwm : 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" {
|
||||
|
||||
HalDigitalHandle initializePWMPort(HalPortHandle port_handle, int32_t* status) {
|
||||
@@ -74,6 +100,91 @@ void freePWMPort(HalDigitalHandle pwm_port_handle, int32_t* status) {
|
||||
|
||||
bool checkPWMChannel(uint8_t pin) { return pin < kNumPWMPins; }
|
||||
|
||||
void setPWMConfig(HalDigitalHandle pwm_port_handle, double max,
|
||||
double deadbandMax, double center, double deadbandMin,
|
||||
double min, int32_t* status) {
|
||||
auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM);
|
||||
if (port == nullptr) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
// calculate the loop time in milliseconds
|
||||
double loopTime =
|
||||
getLoopTiming(status) / (kSystemClockTicksPerMicrosecond * 1e3);
|
||||
if (*status != 0) return;
|
||||
|
||||
int32_t maxPwm = (int32_t)((max - kDefaultPwmCenter) / loopTime +
|
||||
kDefaultPwmStepsDown - 1);
|
||||
int32_t deadbandMaxPwm = (int32_t)(
|
||||
(deadbandMax - kDefaultPwmCenter) / loopTime + kDefaultPwmStepsDown - 1);
|
||||
int32_t centerPwm = (int32_t)((center - kDefaultPwmCenter) / loopTime +
|
||||
kDefaultPwmStepsDown - 1);
|
||||
int32_t deadbandMinPwm = (int32_t)(
|
||||
(deadbandMin - kDefaultPwmCenter) / loopTime + kDefaultPwmStepsDown - 1);
|
||||
int32_t minPwm = (int32_t)((min - kDefaultPwmCenter) / loopTime +
|
||||
kDefaultPwmStepsDown - 1);
|
||||
|
||||
port->maxPwm = maxPwm;
|
||||
port->deadbandMaxPwm = deadbandMaxPwm;
|
||||
port->deadbandMinPwm = deadbandMinPwm;
|
||||
port->centerPwm = centerPwm;
|
||||
port->minPwm = minPwm;
|
||||
port->configSet = true;
|
||||
}
|
||||
|
||||
void setPWMConfigRaw(HalDigitalHandle pwm_port_handle, int32_t maxPwm,
|
||||
int32_t deadbandMaxPwm, int32_t centerPwm,
|
||||
int32_t deadbandMinPwm, int32_t minPwm, int32_t* status) {
|
||||
auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM);
|
||||
if (port == nullptr) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
port->maxPwm = maxPwm;
|
||||
port->deadbandMaxPwm = deadbandMaxPwm;
|
||||
port->deadbandMinPwm = deadbandMinPwm;
|
||||
port->centerPwm = centerPwm;
|
||||
port->minPwm = minPwm;
|
||||
}
|
||||
|
||||
void getPWMConfigRaw(HalDigitalHandle pwm_port_handle, int32_t* maxPwm,
|
||||
int32_t* deadbandMaxPwm, int32_t* centerPwm,
|
||||
int32_t* deadbandMinPwm, int32_t* minPwm,
|
||||
int32_t* status) {
|
||||
auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM);
|
||||
if (port == nullptr) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
*maxPwm = port->maxPwm;
|
||||
*deadbandMaxPwm = port->deadbandMaxPwm;
|
||||
*deadbandMinPwm = port->deadbandMinPwm;
|
||||
*centerPwm = port->centerPwm;
|
||||
*minPwm = port->minPwm;
|
||||
}
|
||||
|
||||
void setPWMEliminateDeadband(HalDigitalHandle pwm_port_handle,
|
||||
uint8_t eliminateDeadband, int32_t* status) {
|
||||
auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM);
|
||||
if (port == nullptr) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
port->eliminateDeadband = eliminateDeadband;
|
||||
}
|
||||
|
||||
uint8_t getPWMEliminateDeadband(HalDigitalHandle pwm_port_handle,
|
||||
int32_t* status) {
|
||||
auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM);
|
||||
if (port == nullptr) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return false;
|
||||
}
|
||||
return port->eliminateDeadband;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set a PWM channel to the desired value. The values range from 0 to 255 and
|
||||
* the period is controlled
|
||||
@@ -82,8 +193,8 @@ bool checkPWMChannel(uint8_t pin) { return pin < kNumPWMPins; }
|
||||
* @param channel The PWM channel to set.
|
||||
* @param value The PWM value to set.
|
||||
*/
|
||||
void setPWM(HalDigitalHandle pwm_port_handle, unsigned short value,
|
||||
int32_t* status) {
|
||||
void setPWMRaw(HalDigitalHandle pwm_port_handle, uint16_t value,
|
||||
int32_t* status) {
|
||||
auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM);
|
||||
if (port == nullptr) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
@@ -97,13 +208,109 @@ void setPWM(HalDigitalHandle pwm_port_handle, unsigned short value,
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Set a PWM channel to the desired scaled value. The values range from -1 to 1
|
||||
* and
|
||||
* the period is controlled
|
||||
* by the PWM Period and MinHigh registers.
|
||||
*
|
||||
* @param channel The PWM channel to set.
|
||||
* @param value The scaled PWM value to set.
|
||||
*/
|
||||
void setPWMSpeed(HalDigitalHandle pwm_port_handle, float speed,
|
||||
int32_t* status) {
|
||||
auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM);
|
||||
if (port == nullptr) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
if (!port->configSet) {
|
||||
*status = INCOMPATIBLE_STATE;
|
||||
return;
|
||||
}
|
||||
|
||||
DigitalPort* dPort = port.get();
|
||||
|
||||
if (speed < -1.0) {
|
||||
speed = -1.0;
|
||||
} else if (speed > 1.0) {
|
||||
speed = 1.0;
|
||||
}
|
||||
|
||||
// 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 = (int32_t)(speed * ((float)GetPositiveScaleFactor(dPort)) +
|
||||
((float)GetMinPositivePwm(dPort)) + 0.5);
|
||||
} else {
|
||||
rawValue = (int32_t)(speed * ((float)GetNegativeScaleFactor(dPort)) +
|
||||
((float)GetMaxNegativePwm(dPort)) + 0.5);
|
||||
}
|
||||
|
||||
if (!((rawValue >= GetMinNegativePwm(dPort)) &&
|
||||
(rawValue <= GetMaxPositivePwm(dPort))) ||
|
||||
rawValue == kPwmDisabled) {
|
||||
*status = HAL_PWM_SCALE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
setPWMRaw(pwm_port_handle, rawValue, status);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set a PWM channel to the desired position value. The values range from 0 to 1
|
||||
* and
|
||||
* the period is controlled
|
||||
* by the PWM Period and MinHigh registers.
|
||||
*
|
||||
* @param channel The PWM channel to set.
|
||||
* @param value The scaled PWM value to set.
|
||||
*/
|
||||
void setPWMPosition(HalDigitalHandle pwm_port_handle, float pos,
|
||||
int32_t* status) {
|
||||
auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM);
|
||||
if (port == nullptr) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return;
|
||||
}
|
||||
if (!port->configSet) {
|
||||
*status = INCOMPATIBLE_STATE;
|
||||
return;
|
||||
}
|
||||
DigitalPort* dPort = port.get();
|
||||
|
||||
if (pos < 0.0) {
|
||||
pos = 0.0;
|
||||
} else if (pos > 1.0) {
|
||||
pos = 1.0;
|
||||
}
|
||||
|
||||
// note, need to perform the multiplication below as floating point before
|
||||
// converting to int
|
||||
uint16_t rawValue = (int32_t)((pos * (float)GetFullRangeScaleFactor(dPort)) +
|
||||
GetMinNegativePwm(dPort));
|
||||
|
||||
if (rawValue == kPwmDisabled) {
|
||||
*status = HAL_PWM_SCALE_ERROR;
|
||||
return;
|
||||
}
|
||||
|
||||
setPWMRaw(pwm_port_handle, rawValue, status);
|
||||
}
|
||||
|
||||
void setPWMDisabled(HalDigitalHandle pwm_port_handle, int32_t* status) {
|
||||
setPWMRaw(pwm_port_handle, kPwmDisabled, status);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get a value from a PWM channel. The values range from 0 to 255.
|
||||
*
|
||||
* @param channel The PWM channel to read from.
|
||||
* @return The raw PWM value.
|
||||
*/
|
||||
unsigned short getPWM(HalDigitalHandle pwm_port_handle, int32_t* status) {
|
||||
uint16_t getPWMRaw(HalDigitalHandle pwm_port_handle, int32_t* status) {
|
||||
auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM);
|
||||
if (port == nullptr) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
@@ -117,6 +324,75 @@ unsigned short getPWM(HalDigitalHandle pwm_port_handle, int32_t* status) {
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get a scaled value from a PWM channel. The values range from -1 to 1.
|
||||
*
|
||||
* @param channel The PWM channel to read from.
|
||||
* @return The scaled PWM value.
|
||||
*/
|
||||
float getPWMSpeed(HalDigitalHandle pwm_port_handle, int32_t* status) {
|
||||
auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM);
|
||||
if (port == nullptr) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return 0;
|
||||
}
|
||||
if (!port->configSet) {
|
||||
*status = INCOMPATIBLE_STATE;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t value = getPWMRaw(pwm_port_handle, status);
|
||||
if (*status != 0) return 0;
|
||||
DigitalPort* dPort = port.get();
|
||||
|
||||
if (value == kPwmDisabled) {
|
||||
return 0.0;
|
||||
} else if (value > GetMaxPositivePwm(dPort)) {
|
||||
return 1.0;
|
||||
} else if (value < GetMinNegativePwm(dPort)) {
|
||||
return -1.0;
|
||||
} else if (value > GetMinPositivePwm(dPort)) {
|
||||
return (float)(value - GetMinPositivePwm(dPort)) /
|
||||
(float)GetPositiveScaleFactor(dPort);
|
||||
} else if (value < GetMaxNegativePwm(dPort)) {
|
||||
return (float)(value - GetMaxNegativePwm(dPort)) /
|
||||
(float)GetNegativeScaleFactor(dPort);
|
||||
} else {
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get a position value from a PWM channel. The values range from 0 to 1.
|
||||
*
|
||||
* @param channel The PWM channel to read from.
|
||||
* @return The scaled PWM value.
|
||||
*/
|
||||
float getPWMPosition(HalDigitalHandle pwm_port_handle, int32_t* status) {
|
||||
auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM);
|
||||
if (port == nullptr) {
|
||||
*status = HAL_HANDLE_ERROR;
|
||||
return 0;
|
||||
}
|
||||
if (!port->configSet) {
|
||||
*status = INCOMPATIBLE_STATE;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t value = getPWMRaw(pwm_port_handle, status);
|
||||
if (*status != 0) return 0;
|
||||
DigitalPort* dPort = port.get();
|
||||
|
||||
if (value < GetMinNegativePwm(dPort)) {
|
||||
return 0.0;
|
||||
} else if (value > GetMaxPositivePwm(dPort)) {
|
||||
return 1.0;
|
||||
} else {
|
||||
return (float)(value - GetMinNegativePwm(dPort)) /
|
||||
(float)GetFullRangeScaleFactor(dPort);
|
||||
}
|
||||
}
|
||||
|
||||
void latchPWMZero(HalDigitalHandle pwm_port_handle, int32_t* status) {
|
||||
auto port = digitalPinHandles.Get(pwm_port_handle, HalHandleEnum::PWM);
|
||||
if (port == nullptr) {
|
||||
|
||||
Reference in New Issue
Block a user