mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
271 lines
10 KiB
C++
271 lines
10 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) FIRST 2016. 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 the root directory of */
|
|
/* the project. */
|
|
/*----------------------------------------------------------------------------*/
|
|
|
|
#include "FPGAEncoder.h"
|
|
|
|
#include "DigitalInternal.h"
|
|
#include "PortsInternal.h"
|
|
#include "handles/LimitedHandleResource.h"
|
|
|
|
using namespace hal;
|
|
|
|
namespace {
|
|
struct Encoder {
|
|
tEncoder* encoder;
|
|
uint32_t index;
|
|
};
|
|
}
|
|
|
|
static const double DECODING_SCALING_FACTOR = 0.25;
|
|
|
|
static LimitedHandleResource<HalFPGAEncoderHandle, Encoder, kNumEncoders,
|
|
HalHandleEnum::FPGAEncoder>
|
|
fpgaEncoderHandles;
|
|
|
|
extern "C" {
|
|
HalFPGAEncoderHandle initializeFPGAEncoder(
|
|
uint8_t port_a_module, uint32_t port_a_pin, bool port_a_analog_trigger,
|
|
uint8_t port_b_module, uint32_t port_b_pin, bool port_b_analog_trigger,
|
|
bool reverseDirection, int32_t* index, int32_t* status) {
|
|
remapDigitalSource(port_a_analog_trigger, port_a_pin, port_a_module);
|
|
remapDigitalSource(port_b_analog_trigger, port_b_pin, port_b_module);
|
|
|
|
auto handle = fpgaEncoderHandles.Allocate();
|
|
if (handle == HAL_INVALID_HANDLE) { // out of resources
|
|
*status = NO_AVAILABLE_RESOURCES;
|
|
return HAL_INVALID_HANDLE;
|
|
}
|
|
|
|
auto encoder = fpgaEncoderHandles.Get(handle);
|
|
if (encoder == nullptr) { // will only error on thread issue
|
|
*status = PARAMETER_OUT_OF_RANGE;
|
|
return HAL_INVALID_HANDLE;
|
|
}
|
|
|
|
encoder->index = static_cast<uint32_t>(getHandleIndex(handle));
|
|
|
|
*index = encoder->index;
|
|
// TODO: if (index == ~0ul) { CloneError(quadEncoders); return; }
|
|
encoder->encoder = tEncoder::create(encoder->index, status);
|
|
encoder->encoder->writeConfig_ASource_Module(port_a_module, status);
|
|
encoder->encoder->writeConfig_ASource_Channel(port_a_pin, status);
|
|
encoder->encoder->writeConfig_ASource_AnalogTrigger(port_a_analog_trigger,
|
|
status);
|
|
encoder->encoder->writeConfig_BSource_Module(port_b_module, status);
|
|
encoder->encoder->writeConfig_BSource_Channel(port_b_pin, status);
|
|
encoder->encoder->writeConfig_BSource_AnalogTrigger(port_b_analog_trigger,
|
|
status);
|
|
encoder->encoder->strobeReset(status);
|
|
encoder->encoder->writeConfig_Reverse(reverseDirection, status);
|
|
encoder->encoder->writeTimerConfig_AverageSize(4, status);
|
|
|
|
return handle;
|
|
}
|
|
|
|
void freeFPGAEncoder(HalFPGAEncoderHandle fpga_encoder_handle,
|
|
int32_t* status) {
|
|
auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
|
|
fpgaEncoderHandles.Free(fpga_encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = PARAMETER_OUT_OF_RANGE;
|
|
return;
|
|
}
|
|
delete encoder->encoder;
|
|
}
|
|
|
|
/**
|
|
* Reset the Encoder distance to zero.
|
|
* Resets the current count to zero on the encoder.
|
|
*/
|
|
void resetFPGAEncoder(HalFPGAEncoderHandle fpga_encoder_handle,
|
|
int32_t* status) {
|
|
auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = PARAMETER_OUT_OF_RANGE;
|
|
return;
|
|
}
|
|
encoder->encoder->strobeReset(status);
|
|
}
|
|
|
|
/**
|
|
* Gets the fpga value from the encoder.
|
|
* The fpga value is the actual count unscaled by the 1x, 2x, or 4x scale
|
|
* factor.
|
|
* @return Current fpga count from the encoder
|
|
*/
|
|
int32_t getFPGAEncoder(HalFPGAEncoderHandle fpga_encoder_handle,
|
|
int32_t* status) {
|
|
auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = PARAMETER_OUT_OF_RANGE;
|
|
return 0;
|
|
}
|
|
return encoder->encoder->readOutput_Value(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 getFPGAEncoderPeriod(HalFPGAEncoderHandle fpga_encoder_handle,
|
|
int32_t* status) {
|
|
auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = PARAMETER_OUT_OF_RANGE;
|
|
return 0.0;
|
|
}
|
|
tEncoder::tTimerOutput output = encoder->encoder->readTimerOutput(status);
|
|
double value;
|
|
if (output.Stalled) {
|
|
// Return infinity
|
|
double zero = 0.0;
|
|
value = 1.0 / zero;
|
|
} else {
|
|
// output.Period is a fixed point number that counts by 2 (24 bits, 25
|
|
// integer bits)
|
|
value = (double)(output.Period << 1) / (double)output.Count;
|
|
}
|
|
double measuredPeriod = value * 2.5e-8;
|
|
return measuredPeriod / DECODING_SCALING_FACTOR;
|
|
}
|
|
|
|
/**
|
|
* 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 setFPGAEncoderMaxPeriod(HalFPGAEncoderHandle fpga_encoder_handle,
|
|
double maxPeriod, int32_t* status) {
|
|
auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = PARAMETER_OUT_OF_RANGE;
|
|
return;
|
|
}
|
|
encoder->encoder->writeTimerConfig_StallPeriod(
|
|
(uint32_t)(maxPeriod * 4.0e8 * DECODING_SCALING_FACTOR), 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 getFPGAEncoderStopped(HalFPGAEncoderHandle fpga_encoder_handle,
|
|
int32_t* status) {
|
|
auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = PARAMETER_OUT_OF_RANGE;
|
|
return false;
|
|
}
|
|
return encoder->encoder->readTimerOutput_Stalled(status) != 0;
|
|
}
|
|
|
|
/**
|
|
* The last direction the encoder value changed.
|
|
* @return The last direction the encoder value changed.
|
|
*/
|
|
bool getFPGAEncoderDirection(HalFPGAEncoderHandle fpga_encoder_handle,
|
|
int32_t* status) {
|
|
auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = PARAMETER_OUT_OF_RANGE;
|
|
return false;
|
|
}
|
|
return encoder->encoder->readOutput_Direction(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 setFPGAEncoderReverseDirection(HalFPGAEncoderHandle fpga_encoder_handle,
|
|
bool reverseDirection, int32_t* status) {
|
|
auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = PARAMETER_OUT_OF_RANGE;
|
|
return;
|
|
}
|
|
encoder->encoder->writeConfig_Reverse(reverseDirection, 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 setFPGAEncoderSamplesToAverage(HalFPGAEncoderHandle fpga_encoder_handle,
|
|
uint32_t samplesToAverage,
|
|
int32_t* status) {
|
|
auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = PARAMETER_OUT_OF_RANGE;
|
|
return;
|
|
}
|
|
if (samplesToAverage < 1 || samplesToAverage > 127) {
|
|
*status = PARAMETER_OUT_OF_RANGE;
|
|
}
|
|
encoder->encoder->writeTimerConfig_AverageSize(samplesToAverage, status);
|
|
}
|
|
|
|
/**
|
|
* 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)
|
|
*/
|
|
uint32_t getFPGAEncoderSamplesToAverage(
|
|
HalFPGAEncoderHandle fpga_encoder_handle, int32_t* status) {
|
|
auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = PARAMETER_OUT_OF_RANGE;
|
|
return 0;
|
|
}
|
|
return encoder->encoder->readTimerConfig_AverageSize(status);
|
|
}
|
|
|
|
/**
|
|
* Set an index source for an encoder, which is an input that resets the
|
|
* encoder's count.
|
|
*/
|
|
void setFPGAEncoderIndexSource(HalFPGAEncoderHandle fpga_encoder_handle,
|
|
uint32_t pin, bool analogTrigger,
|
|
bool activeHigh, bool edgeSensitive,
|
|
int32_t* status) {
|
|
auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = PARAMETER_OUT_OF_RANGE;
|
|
return;
|
|
}
|
|
encoder->encoder->writeConfig_IndexSource_Channel((unsigned char)pin, status);
|
|
encoder->encoder->writeConfig_IndexSource_Module((unsigned char)0, status);
|
|
encoder->encoder->writeConfig_IndexSource_AnalogTrigger(analogTrigger,
|
|
status);
|
|
encoder->encoder->writeConfig_IndexActiveHigh(activeHigh, status);
|
|
encoder->encoder->writeConfig_IndexEdgeSensitive(edgeSensitive, status);
|
|
}
|
|
}
|