/**
* Stops counting pulses on the Encoder device. The value is not changed.
*/
void Encoder::Stop()
{
if (StatusIsFatal()) return;
if (m_counter)
m_counter->Stop();
else
{
tRioStatusCode localStatus = NiFpga_Status_Success;
m_encoder->writeConfig_Enable(0, &localStatus);
wpi_setError(localStatus);
}
}
/**
* Set the edge sensitivity on a down counting source.
* Set the down source to either detect rising edges or falling edges.
*/
void Counter::SetDownSourceEdge(bool risingEdge, bool fallingEdge)
{
if (StatusIsFatal()) return;
if (m_downSource == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "Must set non-NULL DownSource before setting DownSourceEdge");
}
tRioStatusCode localStatus = NiFpga_Status_Success;
m_counter->writeConfig_DownRisingEdge(risingEdge, &localStatus);
m_counter->writeConfig_DownFallingEdge(fallingEdge, &localStatus);
wpi_setError(localStatus);
}
/**
* Reset the Encoder distance to zero.
* Resets the current count to zero on the encoder.
*/
void Encoder::Reset()
{
if (StatusIsFatal()) return;
if (m_counter)
m_counter->Reset();
else
{
tRioStatusCode localStatus = NiFpga_Status_Success;
m_encoder->strobeReset(&localStatus);
wpi_setError(localStatus);
}
}
/**
* Set the accumulator's deadband.
*/
void AnalogChannel::SetAccumulatorDeadband(INT32 deadband)
{
if (StatusIsFatal()) return;
if (m_accumulator == NULL)
{
wpi_setWPIError(NullParameter);
return;
}
tRioStatusCode localStatus = NiFpga_Status_Success;
m_accumulator->writeDeadband(deadband, &localStatus);
wpi_setError(localStatus);
}
/**
* Resets the accumulator to the initial value.
*/
void AnalogChannel::ResetAccumulator()
{
if (StatusIsFatal()) return;
if (m_accumulator == NULL)
{
wpi_setWPIError(NullParameter);
return;
}
tRioStatusCode localStatus = NiFpga_Status_Success;
m_accumulator->strobeReset(&localStatus);
wpi_setError(localStatus);
}
/**
* Get a single particle analysis report.
* Get one (of possibly many) particle analysis reports for an image.
* This version could be more efficient when copying many reports.
* @param particleNumber Which particle analysis report to return.
* @param par the selected particle analysis report
*/
void BinaryImage::GetParticleAnalysisReport(int particleNumber, ParticleAnalysisReport *par)
{
int success;
int numParticles = 0;
success = imaqGetImageSize(m_imaqImage, &par->imageWidth, &par->imageHeight);
wpi_setImaqErrorWithContext(success, "Error getting image size");
if (StatusIsFatal())
return;
success = imaqCountParticles(m_imaqImage, 1, &numParticles);
wpi_setImaqErrorWithContext(success, "Error counting particles");
if (StatusIsFatal())
return;
if (particleNumber >= numParticles)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "particleNumber");
return;
}
par->particleIndex = particleNumber;
// Don't bother measuring the rest of the particle if one fails
bool good = ParticleMeasurement(particleNumber, IMAQ_MT_CENTER_OF_MASS_X, &par->center_mass_x);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_CENTER_OF_MASS_Y, &par->center_mass_y);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_AREA, &par->particleArea);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_BOUNDING_RECT_TOP, &par->boundingRect.top);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_BOUNDING_RECT_LEFT, &par->boundingRect.left);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_BOUNDING_RECT_HEIGHT, &par->boundingRect.height);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_BOUNDING_RECT_WIDTH, &par->boundingRect.width);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_AREA_BY_IMAGE_AREA, &par->particleToImagePercent);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_AREA_BY_PARTICLE_AND_HOLES_AREA, &par->particleQuality);
if (good)
{
/* normalized position (-1 to 1) */
par->center_mass_x_normalized = NormalizeFromRange(par->center_mass_x, par->imageWidth);
par->center_mass_y_normalized = NormalizeFromRange(par->center_mass_y, par->imageHeight);
}
}
/**
* Set the Counter to return reversed sensing on the direction.
* This allows counters to change the direction they are counting in the case of 1X and 2X
* quadrature encoding only. Any other counter mode isn't supported.
* @param reverseDirection true if the value counted should be negated.
*/
void xCounter::SetReverseDirection(bool reverseDirection)
{
if (StatusIsFatal()) return;
tRioStatusCode localStatus = NiFpga_Status_Success;
if (m_counter->readConfig_Mode(&localStatus) == kExternalDirection)
{
if (reverseDirection)
SetDownSourceEdge(true, true);
else
SetDownSourceEdge(false, true);
}
wpi_setError(localStatus);
}
/**
* Implement the PIDSource interface.
*
* @return The current value of the selected source parameter.
*/
double Encoder::PIDGet()
{
if (StatusIsFatal()) return 0.0;
switch (m_pidSource)
{
case kDistance:
return GetDistance();
case kRate:
return GetRate();
default:
return 0.0;
}
}
/**
* Set the upper and lower limits of the analog trigger.
* The limits are given as floating point voltage values.
*/
void AnalogTrigger::SetLimitsVoltage(float lower, float upper)
{
if (StatusIsFatal()) return;
if (lower > upper)
{
wpi_setWPIError(AnalogTriggerLimitOrderError);
}
// TODO: This depends on the averaged setting. Only raw values will work as is.
tRioStatusCode localStatus = NiFpga_Status_Success;
m_trigger->writeLowerLimit(m_analogModule->VoltsToValue(m_channel, lower), &localStatus);
m_trigger->writeUpperLimit(m_analogModule->VoltsToValue(m_channel, upper), &localStatus);
wpi_setError(localStatus);
}
/**
* The scale needed to convert a raw counter value into a number of encoder
* pulses.
*/
double Encoder::DecodingScaleFactor() const {
if (StatusIsFatal()) return 0.0;
switch (m_encodingType) {
case k1X:
return 1.0;
case k2X:
return 0.5;
case k4X:
return 0.25;
default:
return 0.0;
}
}
/**
* Read the number of accumulated values.
*
* Read the count of the accumulated values since the accumulator was last Reset().
*
* @return The number of times samples from the channel were accumulated.
*/
UINT32 AnalogChannel::GetAccumulatorCount()
{
if (StatusIsFatal()) return 0;
if (m_accumulator == NULL)
{
wpi_setWPIError(NullParameter);
return 0;
}
tRioStatusCode localStatus = NiFpga_Status_Success;
UINT32 count = m_accumulator->readOutput_Count(&localStatus);
wpi_setError(localStatus);
return count;
}
/**
* Read the accumulated value.
*
* Read the value that has been accumulating on channel 1.
* The accumulator is attached after the oversample and average engine.
*
* @return The 64-bit value accumulated since the last Reset().
*/
INT64 AnalogChannel::GetAccumulatorValue()
{
if (StatusIsFatal()) return 0;
if (m_accumulator == NULL)
{
wpi_setWPIError(NullParameter);
return 0;
}
tRioStatusCode localStatus = NiFpga_Status_Success;
INT64 value = m_accumulator->readOutput_Value(&localStatus) + m_accumulatorOffset;
wpi_setError(localStatus);
return value;
}
/**
* Set the direction sensing for this encoder.
* This sets the direction sensing on the encoder so that it could count in the correct
* software direction regardless of the mounting.
* @param reverseDirection true if the encoder direction should be reversed
*/
void Encoder::SetReverseDirection(bool reverseDirection)
{
if (StatusIsFatal()) return;
if (m_counter)
{
m_counter->SetReverseDirection(reverseDirection);
}
else
{
int32_t status = 0;
setEncoderReverseDirection(m_encoder, reverseDirection, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
/**
* Sets the maximum period for stopped detection.
* Sets the value that represents the maximum period of the Encoder before it will assume
* that the attached device is stopped. This timeout allows users to determine if the wheels or
* other shaft has stopped rotating.
* This method compensates for the decoding type.
*
* @deprecated Use SetMinRate() in favor of this method. This takes unscaled periods and SetMinRate() scales using value from SetDistancePerPulse().
*
* @param maxPeriod The maximum time between rising and falling edges before the FPGA will
* report the device stopped. This is expressed in seconds.
*/
void Encoder::SetMaxPeriod(double maxPeriod)
{
if (StatusIsFatal()) return;
if (m_counter)
{
m_counter->SetMaxPeriod(maxPeriod * DecodingScaleFactor());
}
else
{
int32_t status = 0;
setEncoderMaxPeriod(m_encoder, maxPeriod, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
/**
* Gets the raw value from the encoder.
* The raw value is the actual count unscaled by the 1x, 2x, or 4x scale
* factor.
* @return Current raw count from the encoder
*/
int32_t Encoder::GetRaw()
{
if (StatusIsFatal()) return 0;
int32_t value;
if (m_counter)
value = m_counter->Get();
else
{
int32_t status = 0;
value = getEncoder(m_encoder, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
return value;
}
/**
* Set the relay state.
*
* Valid values depend on which directions of the relay are controlled by the
* object.
*
* When set to kBothDirections, the relay can be any of the four states:
* 0v-0v, 0v-12v, 12v-0v, 12v-12v
*
* When set to kForwardOnly or kReverseOnly, you can specify the constant for
* the
* direction or you can simply specify kOff and kOn. Using only kOff and
* kOn is
* recommended.
*
* @param value The state to set the relay.
*/
void Relay::Set(Relay::Value value) {
if (StatusIsFatal()) return;
int32_t status = 0;
switch (value) {
case kOff:
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
setRelayForward(m_relay_ports[m_channel], false, &status);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
setRelayReverse(m_relay_ports[m_channel], false, &status);
}
break;
case kOn:
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
setRelayForward(m_relay_ports[m_channel], true, &status);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
setRelayReverse(m_relay_ports[m_channel], true, &status);
}
break;
case kForward:
if (m_direction == kReverseOnly) {
wpi_setWPIError(IncompatibleMode);
break;
}
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
setRelayForward(m_relay_ports[m_channel], true, &status);
}
if (m_direction == kBothDirections) {
setRelayReverse(m_relay_ports[m_channel], false, &status);
}
break;
case kReverse:
if (m_direction == kForwardOnly) {
wpi_setWPIError(IncompatibleMode);
break;
}
if (m_direction == kBothDirections) {
setRelayForward(m_relay_ports[m_channel], false, &status);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
setRelayReverse(m_relay_ports[m_channel], true, &status);
}
break;
}
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Set the direction sensing for this encoder.
* This sets the direction sensing on the encoder so that it could count in the correct
* software direction regardless of the mounting.
* @param reverseDirection true if the encoder direction should be reversed
*/
void Encoder::SetReverseDirection(bool reverseDirection)
{
if (StatusIsFatal()) return;
if (m_counter)
{
m_counter->SetReverseDirection(reverseDirection);
}
else
{
tRioStatusCode localStatus = NiFpga_Status_Success;
m_encoder->writeConfig_Reverse(reverseDirection, &localStatus);
wpi_setError(localStatus);
}
}
/**
* Set the relay state.
*
* Valid values depend on which directions of the relay are controlled by the
* object.
*
* When set to kBothDirections, the relay can be any of the four states:
* 0v-0v, 0v-12v, 12v-0v, 12v-12v
*
* When set to kForwardOnly or kReverseOnly, you can specify the constant for
* the direction or you can simply specify kOff and kOn. Using only kOff and
* kOn is recommended.
*
* @param value The state to set the relay.
*/
void Relay::Set(Relay::Value value) {
if (StatusIsFatal()) return;
int32_t status = 0;
switch (value) {
case kOff:
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
HAL_SetRelay(m_forwardHandle, false, &status);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
HAL_SetRelay(m_reverseHandle, false, &status);
}
break;
case kOn:
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
HAL_SetRelay(m_forwardHandle, true, &status);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
HAL_SetRelay(m_reverseHandle, true, &status);
}
break;
case kForward:
if (m_direction == kReverseOnly) {
wpi_setWPIError(IncompatibleMode);
break;
}
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
HAL_SetRelay(m_forwardHandle, true, &status);
}
if (m_direction == kBothDirections) {
HAL_SetRelay(m_reverseHandle, false, &status);
}
break;
case kReverse:
if (m_direction == kForwardOnly) {
wpi_setWPIError(IncompatibleMode);
break;
}
if (m_direction == kBothDirections) {
HAL_SetRelay(m_forwardHandle, false, &status);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
HAL_SetRelay(m_reverseHandle, true, &status);
}
break;
}
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
/**
* Sets the maximum period for stopped detection.
* Sets the value that represents the maximum period of the Encoder before it will assume
* that the attached device is stopped. This timeout allows users to determine if the wheels or
* other shaft has stopped rotating.
* This method compensates for the decoding type.
*
* @deprecated Use SetMinRate() in favor of this method. This takes unscaled periods and SetMinRate() scales using value from SetDistancePerPulse().
*
* @param maxPeriod The maximum time between rising and falling edges before the FPGA will
* report the device stopped. This is expressed in seconds.
*/
void Encoder::SetMaxPeriod(double maxPeriod)
{
if (StatusIsFatal()) return;
if (m_counter)
{
m_counter->SetMaxPeriod(maxPeriod * DecodingScaleFactor());
}
else
{
tRioStatusCode localStatus = NiFpga_Status_Success;
m_encoder->writeTimerConfig_StallPeriod((UINT32)(maxPeriod * 1.0e6 * DecodingScaleFactor()), &localStatus);
wpi_setError(localStatus);
}
}
/**
* Gets the raw value from the encoder.
* The raw value is the actual count unscaled by the 1x, 2x, or 4x scale
* factor.
* @return Current raw count from the encoder
*/
INT32 Encoder::GetRaw()
{
if (StatusIsFatal()) return 0;
INT32 value;
if (m_counter)
value = m_counter->Get();
else
{
tRioStatusCode localStatus = NiFpga_Status_Success;
value = m_encoder->readOutput_Value(&localStatus);
wpi_setError(localStatus);
}
return value;
}
/**
* Return the actual angle in degrees that the robot is currently facing.
*
* The angle is based on the current accumulator value corrected by the
* oversampling rate, the
* gyro type and the A/D calibration values.
* The angle is continuous, that is it will continue from 360->361 degrees. This
* allows algorithms that wouldn't
* want to see a discontinuity in the gyro output as it sweeps from 360 to 0 on
* the second time around.
*
* @return the current heading of the robot in degrees. This heading is based on
* integration
* of the returned rate from the gyro.
*/
float AnalogGyro::GetAngle() const {
if (StatusIsFatal()) return 0.f;
int64_t rawValue;
uint32_t count;
m_analog->GetAccumulatorOutput(rawValue, count);
int64_t value = rawValue - (int64_t)((float)count * m_offset);
double scaledValue = value * 1e-9 * (double)m_analog->GetLSBWeight() *
(double)(1 << m_analog->GetAverageBits()) /
(m_analog->GetSampleRate() * m_voltsPerDegreePerSecond);
return (float)scaledValue;
}
/**
* Read the current value of the solenoid.
*
* @return The current value of the solenoid.
*/
DoubleSolenoid::Value DoubleSolenoid::Get()
{
if (StatusIsFatal()) {
printf("DoubleSolenoid[%d][%d-%d]::Get"
" status is FATAL, returning kOff\n",
(int)m_moduleNumber, (int)m_forwardChannel,
(int)m_reverseChannel);
return kOff;
}
UINT8 value = GetAll();
if (value & m_forwardMask) return kForward;
if (value & m_reverseMask) return kReverse;
return kOff;
}
/**
* The last direction the encoder value changed.
* @return The last direction the encoder value changed.
*/
bool Encoder::GetDirection()
{
if (StatusIsFatal()) return false;
if (m_counter)
{
return m_counter->GetDirection();
}
else
{
tRioStatusCode localStatus = NiFpga_Status_Success;
bool value = m_encoder->readOutput_Direction(&localStatus);
wpi_setError(localStatus);
return value;
}
}
/**
* Determine if the encoder is stopped.
* Using the MaxPeriod value, a boolean is returned that is true if the encoder is considered
* stopped and false if it is still moving. A stopped encoder is one where the most recent pulse
* width exceeds the MaxPeriod.
* @return True if the encoder is considered stopped.
*/
bool Encoder::GetStopped()
{
if (StatusIsFatal()) return true;
if (m_counter)
{
return m_counter->GetStopped();
}
else
{
tRioStatusCode localStatus = NiFpga_Status_Success;
bool value = m_encoder->readTimerOutput_Stalled(&localStatus) != 0;
wpi_setError(localStatus);
return value;
}
}
/**
* The last direction the encoder value changed.
* @return The last direction the encoder value changed.
*/
bool Encoder::GetDirection()
{
if (StatusIsFatal()) return false;
if (m_counter)
{
return m_counter->GetDirection();
}
else
{
int32_t status = 0;
bool value = getEncoderDirection(m_encoder, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
}
/**
* Set which edge to trigger interrupts on
*
* @param risingEdge
* true to interrupt on rising edge
* @param fallingEdge
* true to interrupt on falling edge
*/
void InterruptableSensorBase::SetUpSourceEdge(bool risingEdge, bool fallingEdge)
{
if (StatusIsFatal()) return;
if (m_interrupt == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "You must call RequestInterrupts before SetUpSourceEdge");
return;
}
if (m_interrupt != NULL)
{
int32_t status = 0;
setInterruptUpSourceEdge(m_interrupt, risingEdge, fallingEdge, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
/**
* Free resources associated with the Digital Input class.
*/
DigitalInput::~DigitalInput()
{
if (StatusIsFatal()) return;
if (m_interrupt != NULL)
{
int32_t status = 0;
cleanInterrupts(m_interrupt, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
m_interrupt = NULL;
m_interrupts->Free(m_interruptIndex);
}
int32_t status = 0;
freeDIO(m_digital_ports[m_channel], &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Returns the period of the most recent pulse.
* Returns the period of the most recent Encoder pulse in seconds.
* This method compensates for the decoding type.
*
* @deprecated Use GetRate() in favor of this method. This returns unscaled periods and GetRate() scales using value from SetDistancePerPulse().
*
* @return Period in seconds of the most recent pulse.
*/
double Encoder::GetPeriod()
{
if (StatusIsFatal()) return 0.0;
if (m_counter)
{
return m_counter->GetPeriod() / DecodingScaleFactor();
}
else
{
int32_t status = 0;
double period = getEncoderPeriod(m_encoder, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return period;
}
}
void DigitalInput::SetUpSourceEdge(bool risingEdge, bool fallingEdge)
{
if (StatusIsFatal()) return;
if (m_interrupt == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "You must call RequestInterrupts before SetUpSourceEdge");
return;
}
tRioStatusCode localStatus = NiFpga_Status_Success;
if (m_interrupt != NULL)
{
m_interrupt->writeConfig_RisingEdge(risingEdge, &localStatus);
m_interrupt->writeConfig_FallingEdge(fallingEdge, &localStatus);
}
wpi_setError(localStatus);
}
/**
* 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()
{
if (StatusIsFatal()) return 0.0;
INT32 value = GetRaw();
if (value < GetMinNegativePwm())
{
return 0.0;
}
else if (value > GetMaxPositivePwm())
{
return 1.0;
}
else
{
return (float)(value - GetMinNegativePwm()) / (float)GetFullRangeScaleFactor();
}
}
