/**
* Common initialization code for Encoders.
* This code allocates resources for Encoders and is common to all constructors.
*
* The counter will start counting immediately.
*
* @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_encodingType = encodingType;
switch (encodingType) {
case k4X: {
m_encodingScale = 4;
if (m_aSource->StatusIsFatal()) {
CloneError(*m_aSource);
return;
}
if (m_bSource->StatusIsFatal()) {
CloneError(*m_bSource);
return;
}
int32_t status = 0;
m_encoder = initializeEncoder(
m_aSource->GetModuleForRouting(), m_aSource->GetChannelForRouting(),
m_aSource->GetAnalogTriggerForRouting(),
m_bSource->GetModuleForRouting(), m_bSource->GetChannelForRouting(),
m_bSource->GetAnalogTriggerForRouting(), reverseDirection, &m_index,
&status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
m_counter = nullptr;
SetMaxPeriod(.5);
break;
}
case k1X:
case k2X: {
m_encodingScale = encodingType == k1X ? 1 : 2;
m_counter = std::make_unique<Counter>(m_encodingType, m_aSource,
m_bSource, reverseDirection);
m_index = m_counter->GetFPGAIndex();
break;
}
default:
wpi_setErrorWithContext(-1, "Invalid encodingType argument");
break;
}
HALReport(HALUsageReporting::kResourceType_Encoder, m_index, encodingType);
LiveWindow::GetInstance().AddSensor("Encoder",
m_aSource->GetChannelForRouting(), this);
}
/**
* Create an instance of a DigitalInput.
* Creates a digital input given a channel. Common creation routine for all
* constructors.
*/
void DigitalInput::InitDigitalInput(uint32_t channel)
{
m_table = NULL;
char buf[64];
if (!CheckDigitalChannel(channel))
{
snprintf(buf, 64, "Digital Channel %d", channel);
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
return;
}
m_channel = channel;
int32_t status = 0;
allocateDIO(m_digital_ports[channel], true, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
LiveWindow::GetInstance()->AddSensor("DigitalInput", channel, this);
HALReport(HALUsageReporting::kResourceType_DigitalInput, channel);
}
/**
* Request one of the 8 interrupts asynchronously on this digital input.
* Request interrupts in asynchronous mode where the user's interrupt handler
* will be
* called when the interrupt fires. Users that want control over the thread
* priority
* should use the synchronous method with their own spawned thread.
* The default is interrupt on rising edges only.
*/
void InterruptableSensorBase::RequestInterrupts(
InterruptHandlerFunction handler, void *param) {
if (StatusIsFatal()) return;
uint32_t index = m_interrupts->Allocate("Async Interrupt");
if (index == std::numeric_limits<uint32_t>::max()) {
CloneError(*m_interrupts);
return;
}
m_interruptIndex = index;
// Creates a manager too
AllocateInterrupts(false);
int32_t status = 0;
requestInterrupts(m_interrupt, GetModuleForRouting(), GetChannelForRouting(),
GetAnalogTriggerForRouting(), &status);
SetUpSourceEdge(true, false);
attachInterruptHandler(m_interrupt, handler, param, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Update the alarm hardware to reflect the current first element in the queue.
* Compute the time the next alarm should occur based on the current time and
* the
* period for the first element in the timer queue.
* WARNING: this method does not do synchronization! It must be called from
* somewhere
* that is taking care of synchronizing access to the queue.
*/
void Notifier::UpdateAlarm() {
if (timerQueueHead != nullptr) {
int32_t status = 0;
// This locking is necessary in order to avoid two things:
// 1) Race condition issues with calling cleanNotifer() and
// updateNotifierAlarm() at the same time.
// 2) Avoid deadlock by making it so that this won't block waiting
// for the mutex to unlock.
// Checking refcount as well is unnecessary, but will not hurt.
if (halMutex.try_lock() && refcount != 0) {
if (m_notifier)
updateNotifierAlarm(m_notifier,
(uint32_t)(timerQueueHead->m_expirationTime * 1e6),
&status);
halMutex.unlock();
}
wpi_setStaticErrorWithContext(timerQueueHead, status,
getHALErrorMessage(status));
}
}
uint16_t CANTalon::GetStickyFaults() const {
uint16_t retval = 0;
int val;
CTR_Code status = CTR_OKAY;
/* temperature */
val = 0;
status = m_impl->GetStckyFault_OverTemp(val);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, getHALErrorMessage(status));
retval |= (val) ? CANSpeedController::kTemperatureFault : 0;
/* voltage */
val = 0;
status = m_impl->GetStckyFault_UnderVoltage(val);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, getHALErrorMessage(status));
retval |= (val) ? CANSpeedController::kBusVoltageFault : 0;
/* fwd-limit-switch */
val = 0;
status = m_impl->GetStckyFault_ForLim(val);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, getHALErrorMessage(status));
retval |= (val) ? CANSpeedController::kFwdLimitSwitch : 0;
/* rev-limit-switch */
val = 0;
status = m_impl->GetStckyFault_RevLim(val);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, getHALErrorMessage(status));
retval |= (val) ? CANSpeedController::kRevLimitSwitch : 0;
/* fwd-soft-limit */
val = 0;
status = m_impl->GetStckyFault_ForSoftLim(val);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, getHALErrorMessage(status));
retval |= (val) ? CANSpeedController::kFwdSoftLimit : 0;
/* rev-soft-limit */
val = 0;
status = m_impl->GetStckyFault_RevSoftLim(val);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, getHALErrorMessage(status));
retval |= (val) ? CANSpeedController::kRevSoftLimit : 0;
return retval;
}
/**
* Free the resources for a timer event.
* All resources will be freed and the timer event will be removed from the
* queue if necessary.
*/
Notifier::~Notifier()
{
{
::std::unique_lock<ReentrantMutex> sync(queueSemaphore);
DeleteFromQueue();
// Delete the static variables when the last one is going away
if (!(--refcount))
{
int32_t status = 0;
cleanNotifier(m_notifier, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
// Acquire the semaphore; this makes certain that the handler is
// not being executed by the interrupt manager.
takeSemaphore(m_handlerSemaphore);
// Delete while holding the semaphore so there can be no race.
deleteSemaphore(m_handlerSemaphore);
}
/**
* Set the bounds on the PWM pulse widths.
* This sets the bounds on the PWM values for a particular type of controller.
* The values
* determine the upper and lower speeds as well as the deadband bracket.
* @param max The max PWM pulse width in ms
* @param deadbandMax The high end of the deadband range pulse width in ms
* @param center The center (off) pulse width in ms
* @param deadbandMin The low end of the deadband pulse width in ms
* @param min The minimum pulse width in ms
*/
void PWM::SetBounds(double max, double deadbandMax, double center,
double deadbandMin, double min) {
// calculate the loop time in milliseconds
int32_t status = 0;
double loopTime =
getLoopTiming(&status) / (kSystemClockTicksPerMicrosecond * 1e3);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
if (StatusIsFatal()) return;
m_maxPwm = (int32_t)((max - kDefaultPwmCenter) / loopTime +
kDefaultPwmStepsDown - 1);
m_deadbandMaxPwm = (int32_t)((deadbandMax - kDefaultPwmCenter) / loopTime +
kDefaultPwmStepsDown - 1);
m_centerPwm = (int32_t)((center - kDefaultPwmCenter) / loopTime +
kDefaultPwmStepsDown - 1);
m_deadbandMinPwm = (int32_t)((deadbandMin - kDefaultPwmCenter) / loopTime +
kDefaultPwmStepsDown - 1);
m_minPwm = (int32_t)((min - kDefaultPwmCenter) / loopTime +
kDefaultPwmStepsDown - 1);
}
/**
* TODO documentation (see CANJaguar.cpp)
*/
void CANTalon::ConfigNeutralMode(NeutralMode mode) {
CTR_Code status = CTR_OKAY;
switch (mode) {
default:
case kNeutralMode_Jumper: /* use default setting in flash based on
webdash/BrakeCal button selection */
status = m_impl->SetOverrideBrakeType(
CanTalonSRX::kBrakeOverride_UseDefaultsFromFlash);
break;
case kNeutralMode_Brake:
status = m_impl->SetOverrideBrakeType(
CanTalonSRX::kBrakeOverride_OverrideBrake);
break;
case kNeutralMode_Coast:
status = m_impl->SetOverrideBrakeType(
CanTalonSRX::kBrakeOverride_OverrideCoast);
break;
}
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
/**
* Free the resources for a timer event.
* All resources will be freed and the timer event will be removed from the
* queue if necessary.
*/
Notifier::~Notifier() {
{
std::lock_guard<priority_recursive_mutex> sync(queueMutex);
DeleteFromQueue();
}
// Delete the static variables when the last one is going away
if (refcount.fetch_sub(1) == 1) {
int32_t status = 0;
{
std::lock_guard<priority_mutex> sync(halMutex);
if (m_notifier) {
cleanNotifier(m_notifier, &status);
m_notifier = nullptr;
}
}
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
// Acquire the mutex; this makes certain that the handler is
// not being executed by the interrupt manager.
std::lock_guard<priority_mutex> lock(m_handlerMutex);
}
/**
* Relay constructor given a channel.
*
* This code initializes the relay and reserves all resources that need to be
* locked. Initially the relay is set to both lines at 0v.
* @param channel The channel number (0-3).
* @param direction The direction that the Relay object will control.
*/
Relay::Relay(uint32_t channel, Relay::Direction direction)
: m_channel(channel), m_direction(direction) {
std::stringstream buf;
Resource::CreateResourceObject(relayChannels,
dio_kNumSystems * kRelayChannels * 2);
if (!SensorBase::CheckRelayChannel(m_channel)) {
buf << "Relay Channel " << m_channel;
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
return;
}
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
buf << "Forward Relay " << m_channel;
if (relayChannels->Allocate(m_channel * 2, buf.str()) == ~0ul) {
CloneError(*relayChannels);
return;
}
HALReport(HALUsageReporting::kResourceType_Relay, m_channel);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
buf << "Reverse Relay " << m_channel;
if (relayChannels->Allocate(m_channel * 2 + 1, buf.str()) == ~0ul) {
CloneError(*relayChannels);
return;
}
HALReport(HALUsageReporting::kResourceType_Relay, m_channel + 128);
}
int32_t status = 0;
setRelayForward(m_relay_ports[m_channel], false, &status);
setRelayReverse(m_relay_ports[m_channel], false, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
LiveWindow::GetInstance().AddActuator("Relay", 1, m_channel, this);
}
/**
* Create a Notifier for timer event notification.
* @param handler The handler is called at the notification time which is set
* using StartSingle or StartPeriodic.
*/
Notifier::Notifier(TimerEventHandler handler, void *param)
{
if (handler == NULL)
wpi_setWPIErrorWithContext(NullParameter, "handler must not be NULL");
m_handler = handler;
m_param = param;
m_periodic = false;
m_expirationTime = 0;
m_period = 0;
m_nextEvent = NULL;
m_queued = false;
m_handlerSemaphore = initializeSemaphore(SEMAPHORE_FULL);
{
::std::unique_lock<ReentrantMutex> sync(queueSemaphore);
// do the first time intialization of static variables
if (refcount == 0)
{
int32_t status = 0;
m_notifier = initializeNotifier(ProcessQueue, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
refcount++;
}
}
/**
* Slow down the PWM signal for old devices.
*
* @param mult The period multiplier to apply to this channel
*/
void PWM::SetPeriodMultiplier(PeriodMultiplier mult) {
if (StatusIsFatal()) return;
int32_t status = 0;
switch (mult) {
case kPeriodMultiplier_4X:
setPWMPeriodScale(m_pwm_ports[m_channel], 3,
&status); // Squelch 3 out of 4 outputs
break;
case kPeriodMultiplier_2X:
setPWMPeriodScale(m_pwm_ports[m_channel], 1,
&status); // Squelch 1 out of 2 outputs
break;
case kPeriodMultiplier_1X:
setPWMPeriodScale(m_pwm_ports[m_channel], 0,
&status); // Don't squelch any outputs
break;
default:
wpi_assert(false);
}
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Check if the FPGA outputs are enabled. The outputs may be disabled if the
* robot is disabled
* or e-stopped, the watchdog has expired, or if the roboRIO browns out.
* @return True if the FPGA outputs are enabled.
*/
bool DriverStation::IsSysActive() const {
int32_t status = 0;
bool retVal = HALGetSystemActive(&status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return retVal;
}
/**
* TODO documentation (see CANJaguar.cpp)
* Configures the soft limit enable (wear leveled persistent memory).
* Also sets the limit switch overrides.
*/
void CANTalon::ConfigLimitMode(LimitMode mode) {
CTR_Code status = CTR_OKAY;
switch (mode) {
case kLimitMode_SwitchInputsOnly: /** Only use switches for limits */
/* turn OFF both limits. SRX has individual enables and polarity for each
* limit switch.*/
status = m_impl->SetForwardSoftEnable(false);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
status = m_impl->SetReverseSoftEnable(false);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/* override enable the limit switches, this circumvents the webdash */
status = m_impl->SetOverrideLimitSwitchEn(
CanTalonSRX::kLimitSwitchOverride_EnableFwd_EnableRev);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
break;
case kLimitMode_SoftPositionLimits: /** Use both switches and soft limits */
/* turn on both limits. SRX has individual enables and polarity for each
* limit switch.*/
status = m_impl->SetForwardSoftEnable(true);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
status = m_impl->SetReverseSoftEnable(true);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/* override enable the limit switches, this circumvents the webdash */
status = m_impl->SetOverrideLimitSwitchEn(
CanTalonSRX::kLimitSwitchOverride_EnableFwd_EnableRev);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
break;
case kLimitMode_SrxDisableSwitchInputs: /** disable both limit switches and
soft limits */
/* turn on both limits. SRX has individual enables and polarity for each
* limit switch.*/
status = m_impl->SetForwardSoftEnable(false);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
status = m_impl->SetReverseSoftEnable(false);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/* override enable the limit switches, this circumvents the webdash */
status = m_impl->SetOverrideLimitSwitchEn(
CanTalonSRX::kLimitSwitchOverride_DisableFwd_DisableRev);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
break;
}
}
AnalogTrigger::~AnalogTrigger()
{
int32_t status = 0;
cleanAnalogTrigger(m_trigger, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Set the feedforward value of the currently selected profile.
*
* @param f Feedforward constant for the currently selected PID profile.
* @see SelectProfileSlot to choose between the two sets of gains.
*/
void CANTalon::SetF(double f) {
CTR_Code status = m_impl->SetFgain(m_profile, f);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
/**
* Set the Izone to a nonzero value to auto clear the integral accumulator
* when the absolute value of CloseLoopError exceeds Izone.
*
* @see SelectProfileSlot to choose between the two sets of gains.
*/
void CANTalon::SetIzone(unsigned iz) {
CTR_Code status = m_impl->SetIzone(m_profile, iz);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
void CANTalon::ClearStickyFaults()
{
CTR_Code status = m_impl->ClearStickyFaults();
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* If sensor and motor are out of phase, sensor can be inverted
* (position and velocity multiplied by -1).
* @see GetPosition and @see GetSpeed.
*/
void CANTalon::SetSensorDirection(bool reverseSensor) {
CTR_Code status = m_impl->SetRevFeedbackSensor(reverseSensor ? 1 : 0);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
/**
* Set the sample rate per channel for all analog channels.
* The maximum rate is 500kS/s divided by the number of channels in use.
* This is 62500 samples/s per channel.
* @param samplesPerSecond The number of samples per second.
*/
void AnalogInput::SetSampleRate(float samplesPerSecond) {
int32_t status = 0;
setAnalogSampleRate(samplesPerSecond, &status);
wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Set the accumulator's deadband.
* @param
*/
void AnalogInput::SetAccumulatorDeadband(int32_t deadband) {
if (StatusIsFatal()) return;
int32_t status = 0;
setAccumulatorDeadband(m_port, deadband, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Set the center value of the accumulator.
*
* The center value is subtracted from each A/D value before it is added to the
* accumulator. This
* is used for the center value of devices like gyros and accelerometers to
* take the device offset into account when integrating.
*
* This center value is based on the output of the oversampled and averaged
* source from the accumulator
* channel. Because of this, any non-zero oversample bits will affect the size
* of the value for this field.
*/
void AnalogInput::SetAccumulatorCenter(int32_t center) {
if (StatusIsFatal()) return;
int32_t status = 0;
setAccumulatorCenter(m_port, center, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Select the feedback device to use in closed-loop
*/
void CANTalon::SetFeedbackDevice(FeedbackDevice device) {
CTR_Code status = m_impl->SetFeedbackDeviceSelect((int)device);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
/**
* Select the feedback device to use in closed-loop
*/
void CANTalon::SetStatusFrameRateMs(StatusFrameRate stateFrame, int periodMs) {
CTR_Code status = m_impl->SetStatusFrameRate((int)stateFrame, periodMs);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
/**
* Check if the system is browned out.
* @return True if the system is browned out
*/
bool DriverStation::IsSysBrownedOut() const {
int32_t status = 0;
bool retVal = HALGetBrownedOut(&status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return retVal;
}
/**
* Get the current sample rate for all channels
*
* @return Sample rate.
*/
float AnalogInput::GetSampleRate() {
int32_t status = 0;
float sampleRate = getAnalogSampleRate(&status);
wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
return sampleRate;
}
/**
* Get the number of averaging bits previously configured.
* This gets the number of averaging bits from the FPGA. The actual number of
* averaged samples is 2^bits.
* The averaging is done automatically in the FPGA.
*
* @return Number of bits of averaging previously configured.
*/
uint32_t AnalogInput::GetAverageBits() const {
int32_t status = 0;
int32_t averageBits = getAnalogAverageBits(m_port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return averageBits;
}
/**
* Clear the accumulator for I gain.
*/
void CANTalon::ClearIaccum() {
CTR_Code status = m_impl->SetParam(CanTalonSRX::ePidIaccum, 0);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
InterruptSource::InterruptSource(uint32_t channel) : m_channel(channel) {
int32_t status = 0;
allocateDIO(m_digital_ports[channel], true, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Set the number of oversample bits.
* This sets the number of oversample bits. The actual number of oversampled
* values is 2^bits.
* Use oversampling to improve the resolution of your measurements at the
* expense of sampling rate.
* The oversampling is done automatically in the FPGA.
*
* @param bits Number of bits of oversampling.
*/
void AnalogInput::SetOversampleBits(uint32_t bits) {
if (StatusIsFatal()) return;
int32_t status = 0;
setAnalogOversampleBits(m_port, bits, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
