mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
439 lines
13 KiB
C++
439 lines
13 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 "HAL/Encoder.h"
|
|
|
|
#include "ChipObject.h"
|
|
#include "EncoderInternal.h"
|
|
#include "FPGAEncoder.h"
|
|
#include "HAL/Counter.h"
|
|
#include "HAL/Errors.h"
|
|
#include "PortsInternal.h"
|
|
#include "handles/LimitedClassedHandleResource.h"
|
|
|
|
using namespace hal;
|
|
|
|
Encoder::Encoder(HalHandle digitalSourceHandleA,
|
|
AnalogTriggerType analogTriggerTypeA,
|
|
HalHandle digitalSourceHandleB,
|
|
AnalogTriggerType analogTriggerTypeB, bool reverseDirection,
|
|
EncoderEncodingType encodingType, int32_t* status) {
|
|
m_encodingType = encodingType;
|
|
switch (encodingType) {
|
|
case HAL_Encoder_k4X: {
|
|
m_encodingScale = 4;
|
|
m_encoder = initializeFPGAEncoder(
|
|
digitalSourceHandleA, analogTriggerTypeA, digitalSourceHandleB,
|
|
analogTriggerTypeB, reverseDirection, &m_index, status);
|
|
if (*status != 0) {
|
|
return;
|
|
}
|
|
m_counter = HAL_INVALID_HANDLE;
|
|
SetMaxPeriod(.5, status);
|
|
break;
|
|
}
|
|
case HAL_Encoder_k1X:
|
|
case HAL_Encoder_k2X: {
|
|
SetupCounter(digitalSourceHandleA, analogTriggerTypeA,
|
|
digitalSourceHandleB, analogTriggerTypeB, reverseDirection,
|
|
encodingType, status);
|
|
|
|
m_encodingScale = encodingType == HAL_Encoder_k1X ? 1 : 2;
|
|
break;
|
|
}
|
|
default:
|
|
*status = PARAMETER_OUT_OF_RANGE;
|
|
return;
|
|
}
|
|
}
|
|
|
|
void Encoder::SetupCounter(HalHandle digitalSourceHandleA,
|
|
AnalogTriggerType analogTriggerTypeA,
|
|
HalHandle digitalSourceHandleB,
|
|
AnalogTriggerType analogTriggerTypeB,
|
|
bool reverseDirection,
|
|
EncoderEncodingType encodingType, int32_t* status) {
|
|
m_encodingScale = encodingType == HAL_Encoder_k1X ? 1 : 2;
|
|
m_counter = initializeCounter(kExternalDirection, &m_index, status);
|
|
if (*status != 0) return;
|
|
setCounterMaxPeriod(m_counter, 0.5, status);
|
|
if (*status != 0) return;
|
|
setCounterUpSource(m_counter, digitalSourceHandleA, analogTriggerTypeA,
|
|
status);
|
|
if (*status != 0) return;
|
|
setCounterDownSource(m_counter, digitalSourceHandleB, analogTriggerTypeB,
|
|
status);
|
|
if (*status != 0) return;
|
|
if (encodingType == HAL_Encoder_k1X) {
|
|
setCounterUpSourceEdge(m_counter, true, false, status);
|
|
setCounterAverageSize(m_counter, 1, status);
|
|
} else {
|
|
setCounterUpSourceEdge(m_counter, true, true, status);
|
|
setCounterAverageSize(m_counter, 2, status);
|
|
}
|
|
setCounterDownSourceEdge(m_counter, reverseDirection, true, status);
|
|
}
|
|
|
|
Encoder::~Encoder() {
|
|
if (m_counter != HAL_INVALID_HANDLE) {
|
|
int32_t status = 0;
|
|
freeCounter(m_counter, &status);
|
|
} else {
|
|
int32_t status = 0;
|
|
freeFPGAEncoder(m_encoder, &status);
|
|
}
|
|
}
|
|
|
|
// CounterBase interface
|
|
int32_t Encoder::Get(int32_t* status) const {
|
|
return (int32_t)(GetRaw(status) * DecodingScaleFactor());
|
|
}
|
|
|
|
int32_t Encoder::GetRaw(int32_t* status) const {
|
|
if (m_counter) {
|
|
return getCounter(m_counter, status);
|
|
} else {
|
|
return getFPGAEncoder(m_encoder, status);
|
|
}
|
|
}
|
|
|
|
int32_t Encoder::GetEncodingScale(int32_t* status) const {
|
|
return m_encodingScale;
|
|
}
|
|
|
|
void Encoder::Reset(int32_t* status) {
|
|
if (m_counter) {
|
|
resetCounter(m_counter, status);
|
|
} else {
|
|
resetFPGAEncoder(m_encoder, status);
|
|
}
|
|
}
|
|
|
|
double Encoder::GetPeriod(int32_t* status) const {
|
|
if (m_counter) {
|
|
return getCounterPeriod(m_counter, status) / DecodingScaleFactor();
|
|
} else {
|
|
return getFPGAEncoderPeriod(m_encoder, status);
|
|
}
|
|
}
|
|
|
|
void Encoder::SetMaxPeriod(double maxPeriod, int32_t* status) {
|
|
if (m_counter) {
|
|
setCounterMaxPeriod(m_counter, maxPeriod, status);
|
|
} else {
|
|
setFPGAEncoderMaxPeriod(m_encoder, maxPeriod, status);
|
|
}
|
|
}
|
|
|
|
bool Encoder::GetStopped(int32_t* status) const {
|
|
if (m_counter) {
|
|
return getCounterStopped(m_counter, status);
|
|
} else {
|
|
return getFPGAEncoderStopped(m_encoder, status);
|
|
}
|
|
}
|
|
|
|
bool Encoder::GetDirection(int32_t* status) const {
|
|
if (m_counter) {
|
|
return getCounterDirection(m_counter, status);
|
|
} else {
|
|
return getFPGAEncoderDirection(m_encoder, status);
|
|
}
|
|
}
|
|
|
|
double Encoder::GetDistance(int32_t* status) const {
|
|
return GetRaw(status) * DecodingScaleFactor() * m_distancePerPulse;
|
|
}
|
|
|
|
double Encoder::GetRate(int32_t* status) const {
|
|
return m_distancePerPulse / GetPeriod(status);
|
|
}
|
|
|
|
void Encoder::SetMinRate(double minRate, int32_t* status) {
|
|
SetMaxPeriod(m_distancePerPulse / minRate, status);
|
|
}
|
|
|
|
void Encoder::SetDistancePerPulse(double distancePerPulse, int32_t* status) {
|
|
m_distancePerPulse = distancePerPulse;
|
|
}
|
|
|
|
void Encoder::SetReverseDirection(bool reverseDirection, int32_t* status) {
|
|
if (m_counter) {
|
|
setCounterReverseDirection(m_counter, reverseDirection, status);
|
|
} else {
|
|
setFPGAEncoderReverseDirection(m_encoder, reverseDirection, status);
|
|
}
|
|
}
|
|
|
|
void Encoder::SetSamplesToAverage(int samplesToAverage, int32_t* status) {
|
|
if (samplesToAverage < 1 || samplesToAverage > 127) {
|
|
*status = PARAMETER_OUT_OF_RANGE;
|
|
return;
|
|
}
|
|
if (m_counter) {
|
|
setCounterSamplesToAverage(m_counter, samplesToAverage, status);
|
|
} else {
|
|
setFPGAEncoderSamplesToAverage(m_encoder, samplesToAverage, status);
|
|
}
|
|
}
|
|
|
|
int32_t Encoder::GetSamplesToAverage(int32_t* status) const {
|
|
if (m_counter) {
|
|
return getCounterSamplesToAverage(m_counter, status);
|
|
} else {
|
|
return getFPGAEncoderSamplesToAverage(m_encoder, status);
|
|
}
|
|
}
|
|
|
|
void Encoder::SetIndexSource(HalHandle digitalSourceHandle,
|
|
AnalogTriggerType analogTriggerType,
|
|
EncoderIndexingType type, int32_t* status) {
|
|
if (m_counter) {
|
|
*status = HAL_COUNTER_NOT_SUPPORTED;
|
|
return;
|
|
}
|
|
bool activeHigh =
|
|
(type == HAL_kResetWhileHigh) || (type == HAL_kResetOnRisingEdge);
|
|
bool edgeSensitive =
|
|
(type == HAL_kResetOnFallingEdge) || (type == HAL_kResetOnRisingEdge);
|
|
setFPGAEncoderIndexSource(m_encoder, digitalSourceHandle, analogTriggerType,
|
|
activeHigh, edgeSensitive, status);
|
|
}
|
|
|
|
double Encoder::DecodingScaleFactor() const {
|
|
switch (m_encodingType) {
|
|
case HAL_Encoder_k1X:
|
|
return 1.0;
|
|
case HAL_Encoder_k2X:
|
|
return 0.5;
|
|
case HAL_Encoder_k4X:
|
|
return 0.25;
|
|
default:
|
|
return 0.0;
|
|
}
|
|
}
|
|
|
|
static LimitedClassedHandleResource<HalEncoderHandle, Encoder,
|
|
kNumEncoders + kNumCounters,
|
|
HalHandleEnum::Encoder>
|
|
encoderHandles;
|
|
|
|
extern "C" {
|
|
HalEncoderHandle initializeEncoder(
|
|
HalHandle digitalSourceHandleA, AnalogTriggerType analogTriggerTypeA,
|
|
HalHandle digitalSourceHandleB, AnalogTriggerType analogTriggerTypeB,
|
|
bool reverseDirection, EncoderEncodingType encodingType, int32_t* status) {
|
|
auto encoder = std::make_shared<Encoder>(
|
|
digitalSourceHandleA, analogTriggerTypeA, digitalSourceHandleB,
|
|
analogTriggerTypeB, reverseDirection, encodingType, status);
|
|
if (*status != 0) return HAL_INVALID_HANDLE; // return in creation error
|
|
auto handle = encoderHandles.Allocate(encoder);
|
|
if (handle == HAL_INVALID_HANDLE) {
|
|
*status = NO_AVAILABLE_RESOURCES;
|
|
return HAL_INVALID_HANDLE;
|
|
}
|
|
return handle;
|
|
}
|
|
|
|
void freeEncoder(HalEncoderHandle encoder_handle, int32_t* status) {
|
|
encoderHandles.Free(encoder_handle);
|
|
}
|
|
|
|
int32_t getEncoder(HalEncoderHandle encoder_handle, int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return 0;
|
|
}
|
|
return encoder->Get(status);
|
|
}
|
|
|
|
int32_t getEncoderRaw(HalEncoderHandle encoder_handle, int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return 0;
|
|
}
|
|
return encoder->GetRaw(status);
|
|
}
|
|
|
|
int32_t getEncoderEncodingScale(HalEncoderHandle encoder_handle,
|
|
int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return 0;
|
|
}
|
|
return encoder->GetEncodingScale(status);
|
|
}
|
|
|
|
void resetEncoder(HalEncoderHandle encoder_handle, int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return;
|
|
}
|
|
encoder->Reset(status);
|
|
}
|
|
|
|
int32_t getEncoderPeriod(HalEncoderHandle encoder_handle, int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return 0;
|
|
}
|
|
return encoder->GetPeriod(status);
|
|
}
|
|
|
|
void setEncoderMaxPeriod(HalEncoderHandle encoder_handle, double maxPeriod,
|
|
int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return;
|
|
}
|
|
encoder->SetMaxPeriod(maxPeriod, status);
|
|
}
|
|
|
|
uint8_t getEncoderStopped(HalEncoderHandle encoder_handle, int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return 0;
|
|
}
|
|
return encoder->GetStopped(status);
|
|
}
|
|
|
|
uint8_t getEncoderDirection(HalEncoderHandle encoder_handle, int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return 0;
|
|
}
|
|
return encoder->GetDirection(status);
|
|
}
|
|
|
|
double getEncoderDistance(HalEncoderHandle encoder_handle, int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return 0;
|
|
}
|
|
return encoder->GetDistance(status);
|
|
}
|
|
|
|
double getEncoderRate(HalEncoderHandle encoder_handle, int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return 0;
|
|
}
|
|
return encoder->GetRate(status);
|
|
}
|
|
|
|
void setEncoderMinRate(HalEncoderHandle encoder_handle, double minRate,
|
|
int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return;
|
|
}
|
|
encoder->SetMinRate(minRate, status);
|
|
}
|
|
|
|
void setEncoderDistancePerPulse(HalEncoderHandle encoder_handle,
|
|
double distancePerPulse, int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return;
|
|
}
|
|
encoder->SetDistancePerPulse(distancePerPulse, status);
|
|
}
|
|
|
|
void setEncoderReverseDirection(HalEncoderHandle encoder_handle,
|
|
uint8_t reverseDirection, int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return;
|
|
}
|
|
encoder->SetReverseDirection(reverseDirection, status);
|
|
}
|
|
|
|
void setEncoderSamplesToAverage(HalEncoderHandle encoder_handle,
|
|
int32_t samplesToAverage, int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return;
|
|
}
|
|
encoder->SetSamplesToAverage(samplesToAverage, status);
|
|
}
|
|
|
|
int32_t getEncoderSamplesToAverage(HalEncoderHandle encoder_handle,
|
|
int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return 0;
|
|
}
|
|
return encoder->GetSamplesToAverage(status);
|
|
}
|
|
|
|
double getEncoderDecodingScaleFactor(HalEncoderHandle encoder_handle,
|
|
int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return 0;
|
|
}
|
|
return encoder->DecodingScaleFactor();
|
|
}
|
|
|
|
double getEncoderDistancePerPulse(HalEncoderHandle encoder_handle,
|
|
int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return 0;
|
|
}
|
|
return encoder->GetDistancePerPulse();
|
|
}
|
|
|
|
EncoderEncodingType getEncoderEncodingType(HalEncoderHandle encoder_handle,
|
|
int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return HAL_Encoder_k4X; // default to k4X
|
|
}
|
|
return encoder->GetEncodingType();
|
|
}
|
|
|
|
void setEncoderIndexSource(HalEncoderHandle encoder_handle,
|
|
HalHandle digitalSourceHandle,
|
|
AnalogTriggerType analogTriggerType,
|
|
EncoderIndexingType type, int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return;
|
|
}
|
|
encoder->SetIndexSource(digitalSourceHandle, analogTriggerType, type, status);
|
|
}
|
|
|
|
int32_t getEncoderFPGAIndex(HalEncoderHandle encoder_handle, int32_t* status) {
|
|
auto encoder = encoderHandles.Get(encoder_handle);
|
|
if (encoder == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return 0;
|
|
}
|
|
return encoder->GetFPGAIndex();
|
|
}
|
|
}
|