mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
58021f7397
AnalogModule and DigitalModule classes still exist, at least until they are refactored into the classes that use them. Change-Id: I5544d5418822f19d54ba0a5d651e64fad8b7b10d
551 lines
16 KiB
C++
551 lines
16 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) FIRST 2008. All Rights Reserved. */
|
|
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
|
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
|
|
/*----------------------------------------------------------------------------*/
|
|
|
|
#include "Encoder.h"
|
|
#include "DigitalInput.h"
|
|
//#include "NetworkCommunication/UsageReporting.h"
|
|
#include "Resource.h"
|
|
#include "WPIErrors.h"
|
|
#include "LiveWindow/LiveWindow.h"
|
|
|
|
/**
|
|
* Common initialization code for Encoders.
|
|
* This code allocates resources for Encoders and is common to all constructors.
|
|
* @param reverseDirection If true, counts down instead of up (this is all relative)
|
|
* @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
|
|
* selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder
|
|
* spec'd value since all rising and falling edges are counted. If 1X or 2X are selected then
|
|
* a counter object will be used and the returned value will either exactly match the spec'd count
|
|
* or be double (2x) the spec'd count.
|
|
*/
|
|
void Encoder::InitEncoder(bool reverseDirection, EncodingType encodingType)
|
|
{
|
|
m_table = NULL;
|
|
m_encodingType = encodingType;
|
|
int32_t index = 0;
|
|
switch (encodingType)
|
|
{
|
|
case k4X:
|
|
{
|
|
if (m_aSource->StatusIsFatal())
|
|
{
|
|
CloneError(m_aSource);
|
|
return;
|
|
}
|
|
if (m_bSource->StatusIsFatal())
|
|
{
|
|
CloneError(m_bSource);
|
|
return;
|
|
}
|
|
int32_t status = 0;
|
|
int32_t index = 0;
|
|
m_encoder = initializeEncoder(m_aSource->GetModuleForRouting(), m_aSource->GetChannelForRouting(),
|
|
m_aSource->GetAnalogTriggerForRouting(),
|
|
m_bSource->GetModuleForRouting(), m_bSource->GetChannelForRouting(),
|
|
m_bSource->GetAnalogTriggerForRouting(),
|
|
reverseDirection, &index, &status);
|
|
wpi_setErrorWithContext(status, getHALErrorMessage(status));
|
|
m_counter = NULL;
|
|
break;
|
|
}
|
|
case k1X:
|
|
case k2X:
|
|
{
|
|
m_counter = new Counter(m_encodingType, m_aSource, m_bSource, reverseDirection);
|
|
index = m_counter->GetIndex();
|
|
break;
|
|
}
|
|
}
|
|
m_distancePerPulse = 1.0;
|
|
m_pidSource = kDistance;
|
|
|
|
HALReport(HALUsageReporting::kResourceType_Encoder, index, encodingType);
|
|
LiveWindow::GetInstance()->AddSensor("Encoder", m_aSource->GetChannelForRouting(), this);
|
|
}
|
|
|
|
/**
|
|
* Encoder constructor.
|
|
* Construct a Encoder given a and b channels.
|
|
* @param aChannel The a channel digital input channel.
|
|
* @param bChannel The b channel digital input channel.
|
|
* @param reverseDirection represents the orientation of the encoder and inverts the output values
|
|
* if necessary so forward represents positive values.
|
|
* @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
|
|
* selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder
|
|
* spec'd value since all rising and falling edges are counted. If 1X or 2X are selected then
|
|
* a counter object will be used and the returned value will either exactly match the spec'd count
|
|
* or be double (2x) the spec'd count.
|
|
*/
|
|
Encoder::Encoder(uint32_t aChannel, uint32_t bChannel, bool reverseDirection, EncodingType encodingType) :
|
|
m_encoder(NULL),
|
|
m_counter(NULL)
|
|
{
|
|
m_aSource = new DigitalInput(aChannel);
|
|
m_bSource = new DigitalInput(bChannel);
|
|
InitEncoder(reverseDirection, encodingType);
|
|
m_allocatedASource = true;
|
|
m_allocatedBSource = true;
|
|
}
|
|
|
|
/**
|
|
* Encoder constructor.
|
|
* Construct a Encoder given a and b channels as digital inputs. This is used in the case
|
|
* where the digital inputs are shared. The Encoder class will not allocate the digital inputs
|
|
* and assume that they already are counted.
|
|
* @param aSource The source that should be used for the a channel.
|
|
* @param bSource the source that should be used for the b channel.
|
|
* @param reverseDirection represents the orientation of the encoder and inverts the output values
|
|
* if necessary so forward represents positive values.
|
|
* @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
|
|
* selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder
|
|
* spec'd value since all rising and falling edges are counted. If 1X or 2X are selected then
|
|
* a counter object will be used and the returned value will either exactly match the spec'd count
|
|
* or be double (2x) the spec'd count.
|
|
*/
|
|
Encoder::Encoder(DigitalSource *aSource, DigitalSource *bSource, bool reverseDirection, EncodingType encodingType) :
|
|
m_encoder(NULL),
|
|
m_counter(NULL)
|
|
{
|
|
m_aSource = aSource;
|
|
m_bSource = bSource;
|
|
m_allocatedASource = false;
|
|
m_allocatedBSource = false;
|
|
if (m_aSource == NULL || m_bSource == NULL)
|
|
wpi_setWPIError(NullParameter);
|
|
else
|
|
InitEncoder(reverseDirection, encodingType);
|
|
}
|
|
|
|
/**
|
|
* Encoder constructor.
|
|
* Construct a Encoder given a and b channels as digital inputs. This is used in the case
|
|
* where the digital inputs are shared. The Encoder class will not allocate the digital inputs
|
|
* and assume that they already are counted.
|
|
* @param aSource The source that should be used for the a channel.
|
|
* @param bSource the source that should be used for the b channel.
|
|
* @param reverseDirection represents the orientation of the encoder and inverts the output values
|
|
* if necessary so forward represents positive values.
|
|
* @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
|
|
* selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder
|
|
* spec'd value since all rising and falling edges are counted. If 1X or 2X are selected then
|
|
* a counter object will be used and the returned value will either exactly match the spec'd count
|
|
* or be double (2x) the spec'd count.
|
|
*/
|
|
Encoder::Encoder(DigitalSource &aSource, DigitalSource &bSource, bool reverseDirection, EncodingType encodingType) :
|
|
m_encoder(NULL),
|
|
m_counter(NULL)
|
|
{
|
|
m_aSource = &aSource;
|
|
m_bSource = &bSource;
|
|
m_allocatedASource = false;
|
|
m_allocatedBSource = false;
|
|
InitEncoder(reverseDirection, encodingType);
|
|
}
|
|
|
|
/**
|
|
* Free the resources for an Encoder.
|
|
* Frees the FPGA resources associated with an Encoder.
|
|
*/
|
|
Encoder::~Encoder()
|
|
{
|
|
if (m_allocatedASource) delete m_aSource;
|
|
if (m_allocatedBSource) delete m_bSource;
|
|
if (m_counter)
|
|
{
|
|
delete m_counter;
|
|
}
|
|
else
|
|
{
|
|
int32_t status = 0;
|
|
freeEncoder(m_encoder, &status);
|
|
wpi_setErrorWithContext(status, getHALErrorMessage(status));
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Start the Encoder.
|
|
* Starts counting pulses on the Encoder device.
|
|
*/
|
|
void Encoder::Start()
|
|
{
|
|
if (StatusIsFatal()) return;
|
|
if (m_counter)
|
|
m_counter->Start();
|
|
else
|
|
{
|
|
int32_t status = 0;
|
|
startEncoder(m_encoder, &status);
|
|
wpi_setErrorWithContext(status, getHALErrorMessage(status));
|
|
}
|
|
}
|
|
|
|
/**
|
|
* 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
|
|
{
|
|
int32_t status = 0;
|
|
stopEncoder(m_encoder, &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;
|
|
}
|
|
|
|
/**
|
|
* Gets the current count.
|
|
* Returns the current count on the Encoder.
|
|
* This method compensates for the decoding type.
|
|
*
|
|
* @return Current count from the Encoder adjusted for the 1x, 2x, or 4x scale factor.
|
|
*/
|
|
int32_t Encoder::Get()
|
|
{
|
|
if (StatusIsFatal()) return 0;
|
|
return (int32_t) (GetRaw() * DecodingScaleFactor());
|
|
}
|
|
|
|
/**
|
|
* 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
|
|
{
|
|
int32_t status = 0;
|
|
resetEncoder(m_encoder, &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 compenstates 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;
|
|
}
|
|
|
|
}
|
|
|
|
/**
|
|
* 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));
|
|
}
|
|
}
|
|
|
|
/**
|
|
* 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
|
|
{
|
|
int32_t status = 0;
|
|
bool value = getEncoderStopped(m_encoder, &status);
|
|
wpi_setErrorWithContext(status, getHALErrorMessage(status));
|
|
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;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* The scale needed to convert a raw counter value into a number of encoder pulses.
|
|
*/
|
|
double Encoder::DecodingScaleFactor()
|
|
{
|
|
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;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Get the distance the robot has driven since the last reset.
|
|
*
|
|
* @return The distance driven since the last reset as scaled by the value from SetDistancePerPulse().
|
|
*/
|
|
double Encoder::GetDistance()
|
|
{
|
|
if (StatusIsFatal()) return 0.0;
|
|
return GetRaw() * DecodingScaleFactor() * m_distancePerPulse;
|
|
}
|
|
|
|
/**
|
|
* Get the current rate of the encoder.
|
|
* Units are distance per second as scaled by the value from SetDistancePerPulse().
|
|
*
|
|
* @return The current rate of the encoder.
|
|
*/
|
|
double Encoder::GetRate()
|
|
{
|
|
if (StatusIsFatal()) return 0.0;
|
|
return (m_distancePerPulse / GetPeriod());
|
|
}
|
|
|
|
/**
|
|
* Set the minimum rate of the device before the hardware reports it stopped.
|
|
*
|
|
* @param minRate The minimum rate. The units are in distance per second as scaled by the value from SetDistancePerPulse().
|
|
*/
|
|
void Encoder::SetMinRate(double minRate)
|
|
{
|
|
if (StatusIsFatal()) return;
|
|
SetMaxPeriod(m_distancePerPulse / minRate);
|
|
}
|
|
|
|
/**
|
|
* Set the distance per pulse for this encoder.
|
|
* This sets the multiplier used to determine the distance driven based on the count value
|
|
* from the encoder.
|
|
* Do not include the decoding type in this scale. The library already compensates for the decoding type.
|
|
* Set this value based on the encoder's rated Pulses per Revolution and
|
|
* factor in gearing reductions following the encoder shaft.
|
|
* This distance can be in any units you like, linear or angular.
|
|
*
|
|
* @param distancePerPulse The scale factor that will be used to convert pulses to useful units.
|
|
*/
|
|
void Encoder::SetDistancePerPulse(double distancePerPulse)
|
|
{
|
|
if (StatusIsFatal()) return;
|
|
m_distancePerPulse = distancePerPulse;
|
|
}
|
|
|
|
/**
|
|
* 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));
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* Set the Samples to Average which specifies the number of samples of the timer to
|
|
* average when calculating the period. Perform averaging to account for
|
|
* mechanical imperfections or as oversampling to increase resolution.
|
|
* @param samplesToAverage The number of samples to average from 1 to 127.
|
|
*/
|
|
void Encoder::SetSamplesToAverage(int samplesToAverage)
|
|
{
|
|
if (samplesToAverage < 1 || samplesToAverage > 127)
|
|
{
|
|
wpi_setWPIErrorWithContext(ParameterOutOfRange, "Average counter values must be between 1 and 127");
|
|
}
|
|
int32_t status = 0;
|
|
switch (m_encodingType) {
|
|
case k4X:
|
|
setEncoderSamplesToAverage(m_encoder, samplesToAverage, &status);
|
|
wpi_setErrorWithContext(status, getHALErrorMessage(status));
|
|
break;
|
|
case k1X:
|
|
case k2X:
|
|
m_counter->SetSamplesToAverage(samplesToAverage);
|
|
break;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Get the Samples to Average which specifies the number of samples of the timer to
|
|
* average when calculating the period. Perform averaging to account for
|
|
* mechanical imperfections or as oversampling to increase resolution.
|
|
* @return SamplesToAverage The number of samples being averaged (from 1 to 127)
|
|
*/
|
|
int Encoder::GetSamplesToAverage()
|
|
{
|
|
int result = 1;
|
|
int32_t status = 0;
|
|
switch (m_encodingType) {
|
|
case k4X:
|
|
result = getEncoderSamplesToAverage(m_encoder, &status);
|
|
wpi_setErrorWithContext(status, getHALErrorMessage(status));
|
|
break;
|
|
case k1X:
|
|
case k2X:
|
|
result = m_counter->GetSamplesToAverage();
|
|
break;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
* Set which parameter of the encoder you are using as a process control variable.
|
|
*
|
|
* @param pidSource An enum to select the parameter.
|
|
*/
|
|
void Encoder::SetPIDSourceParameter(PIDSourceParameter pidSource)
|
|
{
|
|
if (StatusIsFatal()) return;
|
|
m_pidSource = pidSource;
|
|
}
|
|
|
|
/**
|
|
* 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;
|
|
}
|
|
}
|
|
|
|
void Encoder::UpdateTable() {
|
|
if (m_table != NULL) {
|
|
m_table->PutNumber("Speed", GetRate());
|
|
m_table->PutNumber("Distance", GetDistance());
|
|
m_table->PutNumber("Distance per Tick", m_distancePerPulse);
|
|
}
|
|
}
|
|
|
|
void Encoder::StartLiveWindowMode() {
|
|
|
|
}
|
|
|
|
void Encoder::StopLiveWindowMode() {
|
|
|
|
}
|
|
|
|
std::string Encoder::GetSmartDashboardType() {
|
|
if (m_encodingType == k4X)
|
|
return "Quadrature Encoder";
|
|
else
|
|
return "Encoder";
|
|
}
|
|
|
|
void Encoder::InitTable(ITable *subTable) {
|
|
m_table = subTable;
|
|
UpdateTable();
|
|
}
|
|
|
|
ITable * Encoder::GetTable() {
|
|
return m_table;
|
|
}
|