/**
* Set the PWM value based on a position.
*
* This is intended to be used by servos.
*
* @pre SetMaxPositivePwm() called.
* @pre SetMinNegativePwm() called.
*
* @param pos The position to set the servo between 0.0 and 1.0.
*/
void PWM::SetPosition(float pos)
{
if (pos < 0.0)
{
pos = 0.0;
}
else if (pos > 1.0)
{
pos = 1.0;
}
INT32 rawValue;
// note, need to perform the multiplication below as floating point
// before converting to int
rawValue =
(INT32) ((pos * (float)GetFullRangeScaleFactor()) +
GetMinNegativePwm());
wpi_assert((rawValue >= GetMinNegativePwm())
&& (rawValue <= GetMaxPositivePwm()));
wpi_assert(rawValue != kPwmDisabled);
// send the computed pwm value to the FPGA
SetRaw((UINT8) rawValue);
}
/**
* Get the PWM value in terms of a position.
*
* This is intended to be used by servos.
*
* @pre SetMaxPositivePwm() called.
* @pre SetMinNegativePwm() called.
*
* @return The position the servo is set to between 0.0 and 1.0.
*/
float PWM::GetPosition() const {
if (StatusIsFatal()) return 0.0;
int32_t value = GetRaw();
if (value < GetMinNegativePwm()) {
return 0.0;
} else if (value > GetMaxPositivePwm()) {
return 1.0;
} else {
return (float)(value - GetMinNegativePwm()) /
(float)GetFullRangeScaleFactor();
}
}
/**
* 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 a position.
*
* This is intended to be used by servos.
*
* @pre SetMaxPositivePwm() called.
* @pre SetMinNegativePwm() called.
*
* @return The position the servo is set to between 0.0 and 1.0.
*/
float PWM::GetPosition()
{
INT32 value = GetRaw();
if (value < GetMinNegativePwm())
{
return 0.0;
}
else if (value > GetMaxPositivePwm())
{
return 1.0;
}
else
{
return (float)(value - GetMinNegativePwm()) / (float)GetFullRangeScaleFactor();
}
}
/**
* 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;
}
}
/**
* Set the PWM value based on a position.
*
* This is intended to be used by servos.
*
* @pre SetMaxPositivePwm() called.
* @pre SetMinNegativePwm() called.
*
* @param pos The position to set the servo between 0.0 and 1.0.
*/
void PWM::SetPosition(float pos) {
if (StatusIsFatal()) return;
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
unsigned short rawValue =
(int32_t)((pos * (float)GetFullRangeScaleFactor()) + GetMinNegativePwm());
// printf("MinNegPWM: %d FullRangeScaleFactor: %d Raw value: %5d Input
//value: %4.4f\n", GetMinNegativePwm(), GetFullRangeScaleFactor(), rawValue,
//pos);
// wpi_assert((rawValue >= GetMinNegativePwm()) && (rawValue <=
//GetMaxPositivePwm()));
wpi_assert(rawValue != kPwmDisabled);
// send the computed pwm value to the FPGA
SetRaw((unsigned short)rawValue);
}
