/**
* Get the PWM value in terms of speed.
*
* This is intended to be used by speed controllers.
*
* @pre SetMaxPositivePwm() called.
* @pre SetMinPositivePwm() called.
* @pre SetMaxNegativePwm() called.
* @pre SetMinNegativePwm() called.
*
* @return The most recently set speed between -1.0 and 1.0.
*/
float PWM::GetSpeed()
{
if (StatusIsFatal()) return 0.0;
INT32 value = GetRaw();
if (value > GetMaxPositivePwm())
{
return 1.0;
}
else if (value < GetMinNegativePwm())
{
return -1.0;
}
else if (value > GetMinPositivePwm())
{
return (float)(value - GetMinPositivePwm()) / (float)GetPositiveScaleFactor();
}
else if (value < GetMaxNegativePwm())
{
return (float)(value - GetMaxNegativePwm()) / (float)GetNegativeScaleFactor();
}
else
{
return 0.0;
}
}
/**
* Set the PWM value based on a speed.
*
* This is intended to be used by speed controllers.
*
* @pre SetMaxPositivePwm() called.
* @pre SetMinPositivePwm() called.
* @pre SetCenterPwm() called.
* @pre SetMaxNegativePwm() called.
* @pre SetMinNegativePwm() called.
*
* @param speed The speed to set the speed controller between -1.0 and 1.0.
*/
void PWM::SetSpeed(float speed) {
if (StatusIsFatal()) return;
// clamp speed to be in the range 1.0 >= speed >= -1.0
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();
} else if (speed > 0.0) {
rawValue = (int32_t)(speed * ((float)GetPositiveScaleFactor()) +
((float)GetMinPositivePwm()) + 0.5);
} else {
rawValue = (int32_t)(speed * ((float)GetNegativeScaleFactor()) +
((float)GetMaxNegativePwm()) + 0.5);
}
// the above should result in a pwm_value in the valid range
wpi_assert((rawValue >= GetMinNegativePwm()) &&
(rawValue <= GetMaxPositivePwm()));
wpi_assert(rawValue != kPwmDisabled);
// send the computed pwm value to the FPGA
SetRaw(rawValue);
}
/**
* Get the PWM value in terms of speed.
*
* This is intended to be used by speed controllers.
*
* @pre SetMaxPositivePwm() called.
* @pre SetMinPositivePwm() called.
* @pre SetMaxNegativePwm() called.
* @pre SetMinNegativePwm() called.
*
* @return The most recently set speed between -1.0 and 1.0.
*/
float PWM::GetSpeed() const {
if (StatusIsFatal()) return 0.0;
int32_t value = GetRaw();
if (value == PWM::kPwmDisabled) {
return 0.0;
} else if (value > GetMaxPositivePwm()) {
return 1.0;
} else if (value < GetMinNegativePwm()) {
return -1.0;
} else if (value > GetMinPositivePwm()) {
return (float)(value - GetMinPositivePwm()) /
(float)GetPositiveScaleFactor();
} else if (value < GetMaxNegativePwm()) {
return (float)(value - GetMaxNegativePwm()) /
(float)GetNegativeScaleFactor();
} else {
return 0.0;
}
}
