Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
b637b9ee4cb6d3cfe506aeca8475cfa2cc63df66
allwpilib/wpilibc/athena/src/CANTalon.cpp
Thad House b637b9ee4c Prepends all HAL functions with HAL_ (#146)
2016-07-09 00:24:26 -07:00

2087 lines
68 KiB
C++
Raw Blame History

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2014-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 "CANTalon.h"
#include <chrono>
#include <sstream>
#include <thread>
#include "HAL/HAL.h"
#include "LiveWindow/LiveWindow.h"
#include "WPIErrors.h"
/**
* Number of adc engineering units per 0 to 3.3V sweep.
* This is necessary for scaling Analog Position in rotations/RPM.
*/
const double kNativeAdcUnitsPerRotation = 1024.0;
/**
* Number of pulse width engineering units per full rotation.
* This is necessary for scaling Pulse Width Decoded Position in rotations/RPM.
*/
const double kNativePwdUnitsPerRotation = 4096.0;
/**
* Number of minutes per 100ms unit. Useful for scaling velocities
* measured by Talon's 100ms timebase to rotations per minute.
*/
const double kMinutesPer100msUnit = 1.0 / 600.0;
constexpr unsigned int CANTalon::kDelayForSolicitedSignalsUs;
/**
* Constructor for the CANTalon device.
*
* @param deviceNumber The CAN ID of the Talon SRX
*/
CANTalon::CANTalon(int deviceNumber)
: m_deviceNumber(deviceNumber),
m_impl(new CanTalonSRX(deviceNumber)),
m_safetyHelper(new MotorSafetyHelper(this)) {
ApplyControlMode(m_controlMode);
m_impl->SetProfileSlotSelect(m_profile);
LiveWindow::GetInstance()->AddActuator("CANTalon", m_deviceNumber, this);
}
/**
* Constructor for the CANTalon device.
*
* @param deviceNumber The CAN ID of the Talon SRX
* @param controlPeriodMs The period in ms to send the CAN control frame.
* Period is bounded to [1ms,95ms].
*/
CANTalon::CANTalon(int deviceNumber, int controlPeriodMs)
: m_deviceNumber(deviceNumber),
m_impl(new CanTalonSRX(deviceNumber, controlPeriodMs)),
m_safetyHelper(new MotorSafetyHelper(this)) {
ApplyControlMode(m_controlMode);
m_impl->SetProfileSlotSelect(m_profile);
LiveWindow::GetInstance()->AddActuator("CANTalon", m_deviceNumber, this);
}
CANTalon::~CANTalon() {
if (m_table != nullptr) m_table->RemoveTableListener(this);
if (m_hasBeenMoved) return;
Disable();
}
/**
* Write out the PID value as seen in the PIDOutput base object.
*
* @deprecated Call Set instead.
*
* @param output Write out the PercentVbus value as was computed by the
* PIDController
*/
void CANTalon::PIDWrite(float output) {
if (GetControlMode() == kPercentVbus) {
Set(output);
} else {
wpi_setWPIErrorWithContext(IncompatibleMode,
"PID only supported in PercentVbus mode");
}
}
/**
* Retrieve the current sensor value. Equivalent to Get().
*
* @return The current sensor value of the Talon.
*/
double CANTalon::PIDGet() { return Get(); }
/**
* Gets the current status of the Talon (usually a sensor value).
*
* In Current mode: returns output current.
* In Speed mode: returns current speed.
* In Position mode: returns current sensor position.
* In PercentVbus and Follower modes: returns current applied throttle.
*
* @return The current sensor value of the Talon.
*/
float CANTalon::Get() const {
int value;
switch (m_controlMode) {
case kVoltage:
return GetOutputVoltage();
case kCurrent:
return GetOutputCurrent();
case kSpeed:
m_impl->GetSensorVelocity(value);
return ScaleNativeUnitsToRpm(m_feedbackDevice, value);
case kPosition:
m_impl->GetSensorPosition(value);
return ScaleNativeUnitsToRotations(m_feedbackDevice, value);
case kPercentVbus:
case kFollower:
default:
m_impl->GetAppliedThrottle(value);
return (float)value / 1023.0;
}
}
/**
* Sets the appropriate output on the talon, depending on the mode.
*
* In PercentVbus, the output is between -1.0 and 1.0, with 0.0 as stopped.
* In Voltage mode, output value is in volts.
* In Current mode, output value is in amperes.
* In Speed mode, output value is in position change / 10ms.
* In Position mode, output value is in encoder ticks or an analog value,
* depending on the sensor.
* In Follower mode, the output value is the integer device ID of the talon to
* duplicate.
*
* @param outputValue The setpoint value, as described above.
* @see SelectProfileSlot to choose between the two sets of gains.
*/
void CANTalon::Set(float value) {
/* feed safety helper since caller just updated our output */
m_safetyHelper->Feed();
if (m_stopped) {
EnableControl();
m_stopped = false;
}
if (m_controlEnabled) {
m_setPoint = value; /* cache set point for GetSetpoint() */
CTR_Code status = CTR_OKAY;
switch (m_controlMode) {
case CANSpeedController::kPercentVbus: {
m_impl->Set(m_isInverted ? -value : value);
status = CTR_OKAY;
} break;
case CANSpeedController::kFollower: {
status = m_impl->SetDemand((int)value);
} break;
case CANSpeedController::kVoltage: {
// Voltage is an 8.8 fixed point number.
int volts = int((m_isInverted ? -value : value) * 256);
status = m_impl->SetDemand(volts);
} break;
case CANSpeedController::kSpeed:
/* if the caller has provided scaling info, apply it */
status = m_impl->SetDemand(ScaleVelocityToNativeUnits(
m_feedbackDevice, m_isInverted ? -value : value));
break;
case CANSpeedController::kPosition:
status = m_impl->SetDemand(
ScaleRotationsToNativeUnits(m_feedbackDevice, value));
break;
case CANSpeedController::kCurrent: {
double milliamperes = (m_isInverted ? -value : value) * 1000.0; /* mA*/
status = m_impl->SetDemand(milliamperes);
} break;
case CANSpeedController::kMotionProfile: {
status = m_impl->SetDemand((int)value);
} break;
default:
wpi_setWPIErrorWithContext(
IncompatibleMode,
"The CAN Talon does not support this control mode.");
break;
}
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
status = m_impl->SetModeSelect(m_sendMode);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
}
/**
* Sets the setpoint to value. Equivalent to Set().
*/
void CANTalon::SetSetpoint(float value) { Set(value); }
/**
* Resets the integral term and disables the controller.
*/
void CANTalon::Reset() {
ClearIaccum();
Disable();
}
/**
* Disables control of the talon, causing the motor to brake or coast
* depending on its mode (see the Talon SRX Software Reference manual
* for more information).
*/
void CANTalon::Disable() {
m_impl->SetModeSelect((int)CANTalon::kDisabled);
m_controlEnabled = false;
}
/**
* Enables control of the Talon, allowing the motor to move.
*/
void CANTalon::EnableControl() {
SetControlMode(m_controlMode);
m_controlEnabled = true;
}
/**
* Enables control of the Talon, allowing the motor to move.
*/
void CANTalon::Enable() { EnableControl(); }
/**
* @return Whether the Talon is currently enabled.
*/
bool CANTalon::IsControlEnabled() const { return m_controlEnabled; }
/**
* @return Whether the Talon is currently enabled.
*/
bool CANTalon::IsEnabled() const { return IsControlEnabled(); }
/**
* @param p Proportional constant to use in PID loop.
* @see SelectProfileSlot to choose between the two sets of gains.
*/
void CANTalon::SetP(double p) {
CTR_Code status = m_impl->SetPgain(m_profile, p);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* Set the integration constant of the currently selected profile.
*
* @param i Integration constant for the currently selected PID profile.
* @see SelectProfileSlot to choose between the two sets of gains.
*/
void CANTalon::SetI(double i) {
CTR_Code status = m_impl->SetIgain(m_profile, i);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* Set the derivative constant of the currently selected profile.
*
* @param d Derivative constant for the currently selected PID profile.
* @see SelectProfileSlot to choose between the two sets of gains.
*/
void CANTalon::SetD(double d) {
CTR_Code status = m_impl->SetDgain(m_profile, d);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(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, HAL_GetErrorMessage(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, HAL_GetErrorMessage(status));
}
}
/**
* SRX has two available slots for PID.
* @param slotIdx one or zero depending on which slot caller wants.
*/
void CANTalon::SelectProfileSlot(int slotIdx) {
m_profile = (slotIdx == 0) ? 0 : 1; /* only get two slots for now */
CTR_Code status = m_impl->SetProfileSlotSelect(m_profile);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* Sets control values for closed loop control.
*
* @param p Proportional constant.
* @param i Integration constant.
* @param d Differential constant.
*
* This function does not modify F-gain. Considerable passing a zero for f
* using the four-parameter function.
*/
void CANTalon::SetPID(double p, double i, double d) {
SetP(p);
SetI(i);
SetD(d);
}
/**
* Sets control values for closed loop control.
*
* @param p Proportional constant.
* @param i Integration constant.
* @param d Differential constant.
* @param f Feedforward constant.
*/
void CANTalon::SetPID(double p, double i, double d, double f) {
SetP(p);
SetI(i);
SetD(d);
SetF(f);
}
/**
* Select the feedback device to use in closed-loop
*/
void CANTalon::SetFeedbackDevice(FeedbackDevice feedbackDevice) {
/* save the selection so that future setters/getters know which scalars to
* apply
*/
m_feedbackDevice = feedbackDevice;
/* pass feedback to actual CAN frame */
CTR_Code status = m_impl->SetFeedbackDeviceSelect((int)feedbackDevice);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(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, HAL_GetErrorMessage(status));
}
}
/**
* Get the current proportional constant.
*
* @return double proportional constant for current profile.
* @see SelectProfileSlot to choose between the two sets of gains.
*/
double CANTalon::GetP() const {
CanTalonSRX::param_t param = m_profile ? CanTalonSRX::eProfileParamSlot1_P
: CanTalonSRX::eProfileParamSlot0_P;
// Update the info in m_impl.
CTR_Code status = m_impl->RequestParam(param);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
// small yield for getting response
std::this_thread::sleep_for(
std::chrono::microseconds(kDelayForSolicitedSignalsUs));
double p;
status = m_impl->GetPgain(m_profile, p);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return p;
}
/**
* TODO documentation (see CANJaguar.cpp)
* @see SelectProfileSlot to choose between the two sets of gains.
*/
double CANTalon::GetI() const {
CanTalonSRX::param_t param = m_profile ? CanTalonSRX::eProfileParamSlot1_I
: CanTalonSRX::eProfileParamSlot0_I;
// Update the info in m_impl.
CTR_Code status = m_impl->RequestParam(param);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
// small yield for getting response
std::this_thread::sleep_for(
std::chrono::microseconds(kDelayForSolicitedSignalsUs));
double i;
status = m_impl->GetIgain(m_profile, i);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return i;
}
/**
* TODO documentation (see CANJaguar.cpp)
* @see SelectProfileSlot to choose between the two sets of gains.
*/
double CANTalon::GetD() const {
CanTalonSRX::param_t param = m_profile ? CanTalonSRX::eProfileParamSlot1_D
: CanTalonSRX::eProfileParamSlot0_D;
// Update the info in m_impl.
CTR_Code status = m_impl->RequestParam(param);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
// small yield for getting response
std::this_thread::sleep_for(
std::chrono::microseconds(kDelayForSolicitedSignalsUs));
double d;
status = m_impl->GetDgain(m_profile, d);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return d;
}
/**
*
* @see SelectProfileSlot to choose between the two sets of gains.
*/
double CANTalon::GetF() const {
CanTalonSRX::param_t param = m_profile ? CanTalonSRX::eProfileParamSlot1_F
: CanTalonSRX::eProfileParamSlot0_F;
// Update the info in m_impl.
CTR_Code status = m_impl->RequestParam(param);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
// small yield for getting response
std::this_thread::sleep_for(
std::chrono::microseconds(kDelayForSolicitedSignalsUs));
double f;
status = m_impl->GetFgain(m_profile, f);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return f;
}
/**
* @see SelectProfileSlot to choose between the two sets of gains.
*/
int CANTalon::GetIzone() const {
CanTalonSRX::param_t param = m_profile
? CanTalonSRX::eProfileParamSlot1_IZone
: CanTalonSRX::eProfileParamSlot0_IZone;
// Update the info in m_impl.
CTR_Code status = m_impl->RequestParam(param);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
std::this_thread::sleep_for(
std::chrono::microseconds(kDelayForSolicitedSignalsUs));
int iz;
status = m_impl->GetIzone(m_profile, iz);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return iz;
}
/**
* @return the current setpoint; ie, whatever was last passed to Set().
*/
double CANTalon::GetSetpoint() const { return m_setPoint; }
/**
* Returns the voltage coming in from the battery.
*
* @return The input voltage in volts.
*/
float CANTalon::GetBusVoltage() const {
double voltage;
CTR_Code status = m_impl->GetBatteryV(voltage);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return voltage;
}
/**
* @return The voltage being output by the Talon, in Volts.
*/
float CANTalon::GetOutputVoltage() const {
int throttle11;
CTR_Code status = m_impl->GetAppliedThrottle(throttle11);
float voltage = GetBusVoltage() * (float(throttle11) / 1023.0);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return voltage;
}
/**
* Returns the current going through the Talon, in Amperes.
*/
float CANTalon::GetOutputCurrent() const {
double current;
CTR_Code status = m_impl->GetCurrent(current);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return current;
}
/**
* Returns temperature of Talon, in degrees Celsius.
*/
float CANTalon::GetTemperature() const {
double temp;
CTR_Code status = m_impl->GetTemp(temp);
if (temp != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return temp;
}
/**
* Set the position value of the selected sensor. This is useful for zero-ing
* quadrature encoders.
*
* Continuous sensors (like analog encoderes) can also partially be set (the
* portion of the postion based on overflows).
*/
void CANTalon::SetPosition(double pos) {
int32_t nativePos = ScaleRotationsToNativeUnits(m_feedbackDevice, pos);
CTR_Code status = m_impl->SetSensorPosition(nativePos);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* TODO documentation (see CANJaguar.cpp)
*
* @return The position of the sensor currently providing feedback.
* When using analog sensors, 0 units corresponds to 0V, 1023
* units corresponds to 3.3V.
* When using an analog encoder (wrapping around 1023 => 0 is
* possible) the units are still 3.3V per 1023 units.
* When using quadrature, each unit is a quadrature edge (4X) mode.
*/
double CANTalon::GetPosition() const {
int32_t position;
CTR_Code status = m_impl->GetSensorPosition(position);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return ScaleNativeUnitsToRotations(m_feedbackDevice, position);
}
/**
* 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, HAL_GetErrorMessage(status));
}
}
/**
* Flips the sign (multiplies by negative one) the throttle values going into
* the motor on the talon in closed loop modes. Typically the application
* should use SetSensorDirection to keep sensor and motor in phase.
* @see SetSensorDirection
* However this routine is helpful for reversing the motor direction
* when Talon is in slave mode, or when using a single-direction position
* sensor in a closed-loop mode.
*
* @param reverseOutput True if motor output should be flipped; False if not.
*/
void CANTalon::SetClosedLoopOutputDirection(bool reverseOutput) {
CTR_Code status = m_impl->SetRevMotDuringCloseLoopEn(reverseOutput ? 1 : 0);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* Returns the current error in the controller.
*
* @return the difference between the setpoint and the sensor value.
*/
int CANTalon::GetClosedLoopError() const {
int error;
/* retrieve the closed loop error in native units */
CTR_Code status = m_impl->GetCloseLoopErr(error);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return error;
}
/**
* Set the allowable closed loop error.
*
* @param allowableCloseLoopError allowable closed loop error for selected
* profile.
*
* Units: mA for Curent closed loop.
* Talon Native Units for position and velocity.
*/
void CANTalon::SetAllowableClosedLoopErr(uint32_t allowableCloseLoopError) {
/* grab param enum */
CanTalonSRX::param_t param;
if (m_profile == 1) {
param = CanTalonSRX::eProfileParamSlot1_AllowableClosedLoopErr;
} else {
param = CanTalonSRX::eProfileParamSlot0_AllowableClosedLoopErr;
}
/* send allowable close loop er in native units */
ConfigSetParameter(param, allowableCloseLoopError);
}
/**
* TODO documentation (see CANJaguar.cpp)
*
* @returns The speed of the sensor currently providing feedback.
*
* The speed units will be in the sensor's native ticks per 100ms.
*
* For analog sensors, 3.3V corresponds to 1023 units. So a speed of 200
* equates to ~0.645 dV per 100ms or 6.451 dV per second. If this is an analog
* encoder, that likely means 1.9548 rotations per sec. For quadrature
* encoders, each unit corresponds a quadrature edge (4X). So a 250 count
* encoder will produce 1000 edge events per rotation. An example speed of 200
* would then equate to 20% of a rotation per 100ms, or 10 rotations per second.
*/
double CANTalon::GetSpeed() const {
int32_t speed;
CTR_Code status = m_impl->GetSensorVelocity(speed);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return ScaleNativeUnitsToRpm(m_feedbackDevice, speed);
}
/**
* Get the position of whatever is in the analog pin of the Talon, regardless of
* whether it is actually being used for feedback.
*
* @returns The 24bit analog value. The bottom ten bits is the ADC (0 - 1023)
* on the analog pin of the Talon. The upper 14 bits tracks the
* overflows and underflows (continuous sensor).
*/
int CANTalon::GetAnalogIn() const {
int position;
CTR_Code status = m_impl->GetAnalogInWithOv(position);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return position;
}
void CANTalon::SetAnalogPosition(int newPosition) {
CTR_Code status = m_impl->SetParam(CanTalonSRX::eAinPosition, newPosition);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* Get the position of whatever is in the analog pin of the Talon, regardless of
* whether it is actually being used for feedback.
*
* @returns The ADC (0 - 1023) on analog pin of the Talon.
*/
int CANTalon::GetAnalogInRaw() const { return GetAnalogIn() & 0x3FF; }
/**
* Get the position of whatever is in the analog pin of the Talon, regardless of
* whether it is actually being used for feedback.
*
* @returns The value (0 - 1023) on the analog pin of the Talon.
*/
int CANTalon::GetAnalogInVel() const {
int vel;
CTR_Code status = m_impl->GetAnalogInVel(vel);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return vel;
}
/**
* Get the position of whatever is in the analog pin of the Talon, regardless of
* whether it is actually being used for feedback.
*
* @returns The value (0 - 1023) on the analog pin of the Talon.
*/
int CANTalon::GetEncPosition() const {
int position;
CTR_Code status = m_impl->GetEncPosition(position);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return position;
}
void CANTalon::SetEncPosition(int newPosition) {
CTR_Code status = m_impl->SetParam(CanTalonSRX::eEncPosition, newPosition);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* Get the position of whatever is in the analog pin of the Talon, regardless of
* whether it is actually being used for feedback.
*
* @returns The value (0 - 1023) on the analog pin of the Talon.
*/
int CANTalon::GetEncVel() const {
int vel;
CTR_Code status = m_impl->GetEncVel(vel);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return vel;
}
int CANTalon::GetPulseWidthPosition() const {
int param;
CTR_Code status = m_impl->GetPulseWidthPosition(param);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
return param;
}
void CANTalon::SetPulseWidthPosition(int newPosition) {
CTR_Code status = m_impl->SetParam(CanTalonSRX::ePwdPosition, newPosition);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
int CANTalon::GetPulseWidthVelocity() const {
int param;
CTR_Code status = m_impl->GetPulseWidthVelocity(param);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
return param;
}
int CANTalon::GetPulseWidthRiseToFallUs() const {
int param;
CTR_Code status = m_impl->GetPulseWidthRiseToFallUs(param);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
return param;
}
int CANTalon::GetPulseWidthRiseToRiseUs() const {
int param;
CTR_Code status = m_impl->GetPulseWidthRiseToRiseUs(param);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
return param;
}
/**
* @param which feedback sensor to check it if is connected.
* @return status of caller's specified sensor type.
*/
CANTalon::FeedbackDeviceStatus CANTalon::IsSensorPresent(
FeedbackDevice feedbackDevice) const {
FeedbackDeviceStatus retval = FeedbackStatusUnknown;
int param;
/* detecting sensor health depends on which sensor caller cares about */
switch (feedbackDevice) {
case QuadEncoder:
case AnalogPot:
case AnalogEncoder:
case EncRising:
case EncFalling:
/* no real good way to tell if these sensor
are actually present so return status unknown. */
break;
case PulseWidth:
case CtreMagEncoder_Relative:
case CtreMagEncoder_Absolute:
/* all of these require pulse width signal to be present. */
CTR_Code status = m_impl->IsPulseWidthSensorPresent(param);
if (status != CTR_OKAY) {
/* we're not getting status info, signal unknown status */
} else {
/* param is updated */
if (param) {
/* pulse signal is present, sensor must be working since it always
generates a pulse waveform.*/
retval = FeedbackStatusPresent;
} else {
/* no pulse present, sensor disconnected */
retval = FeedbackStatusNotPresent;
}
}
break;
}
return retval;
}
/**
* @return IO level of QUADA pin.
*/
int CANTalon::GetPinStateQuadA() const {
int retval;
CTR_Code status = m_impl->GetQuadApin(retval);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return retval;
}
/**
* @return IO level of QUADB pin.
*/
int CANTalon::GetPinStateQuadB() const {
int retval;
CTR_Code status = m_impl->GetQuadBpin(retval);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return retval;
}
/**
* @return IO level of QUAD Index pin.
*/
int CANTalon::GetPinStateQuadIdx() const {
int retval;
CTR_Code status = m_impl->GetQuadIdxpin(retval);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return retval;
}
/**
* @return '1' iff forward limit switch is closed, 0 iff switch is open.
* This function works regardless if limit switch feature is enabled.
*/
int CANTalon::IsFwdLimitSwitchClosed() const {
int retval;
CTR_Code status = m_impl->GetLimitSwitchClosedFor(
retval); /* rename this func, '1' => open, '0' => closed */
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return retval ? 0 : 1;
}
/**
* @return '1' iff reverse limit switch is closed, 0 iff switch is open.
* This function works regardless if limit switch feature is enabled.
*/
int CANTalon::IsRevLimitSwitchClosed() const {
int retval;
CTR_Code status = m_impl->GetLimitSwitchClosedRev(
retval); /* rename this func, '1' => open, '0' => closed */
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return retval ? 0 : 1;
}
/*
* Simple accessor for tracked rise eventso index pin.
* @return number of rising edges on idx pin.
*/
int CANTalon::GetNumberOfQuadIdxRises() const {
int rises;
CTR_Code status = m_impl->GetEncIndexRiseEvents(
rises); /* rename this func, '1' => open, '0' => closed */
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return rises;
}
/*
* @param rises integral value to set into index-rises register. Great way to
* zero the index count.
*/
void CANTalon::SetNumberOfQuadIdxRises(int rises) {
CTR_Code status = m_impl->SetParam(
CanTalonSRX::eEncIndexRiseEvents,
rises); /* rename this func, '1' => open, '0' => closed */
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* TODO documentation (see CANJaguar.cpp)
*/
bool CANTalon::GetForwardLimitOK() const {
int limSwit = 0;
int softLim = 0;
CTR_Code status = CTR_OKAY;
status = m_impl->GetFault_ForSoftLim(softLim);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
status = m_impl->GetFault_ForLim(limSwit);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
/* If either fault is asserted, signal caller we are disabled (with false?) */
return (softLim | limSwit) ? false : true;
}
/**
* TODO documentation (see CANJaguar.cpp)
*/
bool CANTalon::GetReverseLimitOK() const {
int limSwit = 0;
int softLim = 0;
CTR_Code status = CTR_OKAY;
status = m_impl->GetFault_RevSoftLim(softLim);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
status = m_impl->GetFault_RevLim(limSwit);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
/* If either fault is asserted, signal caller we are disabled (with false?) */
return (softLim | limSwit) ? false : true;
}
/**
* TODO documentation (see CANJaguar.cpp)
*/
uint16_t CANTalon::GetFaults() const {
uint16_t retval = 0;
int val;
CTR_Code status = CTR_OKAY;
/* temperature */
val = 0;
status = m_impl->GetFault_OverTemp(val);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
retval |= (val) ? CANSpeedController::kTemperatureFault : 0;
/* voltage */
val = 0;
status = m_impl->GetFault_UnderVoltage(val);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
retval |= (val) ? CANSpeedController::kBusVoltageFault : 0;
/* fwd-limit-switch */
val = 0;
status = m_impl->GetFault_ForLim(val);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
retval |= (val) ? CANSpeedController::kFwdLimitSwitch : 0;
/* rev-limit-switch */
val = 0;
status = m_impl->GetFault_RevLim(val);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
retval |= (val) ? CANSpeedController::kRevLimitSwitch : 0;
/* fwd-soft-limit */
val = 0;
status = m_impl->GetFault_ForSoftLim(val);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
retval |= (val) ? CANSpeedController::kFwdSoftLimit : 0;
/* rev-soft-limit */
val = 0;
status = m_impl->GetFault_RevSoftLim(val);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
retval |= (val) ? CANSpeedController::kRevSoftLimit : 0;
return retval;
}
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, HAL_GetErrorMessage(status));
retval |= (val) ? CANSpeedController::kTemperatureFault : 0;
/* voltage */
val = 0;
status = m_impl->GetStckyFault_UnderVoltage(val);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
retval |= (val) ? CANSpeedController::kBusVoltageFault : 0;
/* fwd-limit-switch */
val = 0;
status = m_impl->GetStckyFault_ForLim(val);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
retval |= (val) ? CANSpeedController::kFwdLimitSwitch : 0;
/* rev-limit-switch */
val = 0;
status = m_impl->GetStckyFault_RevLim(val);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
retval |= (val) ? CANSpeedController::kRevLimitSwitch : 0;
/* fwd-soft-limit */
val = 0;
status = m_impl->GetStckyFault_ForSoftLim(val);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
retval |= (val) ? CANSpeedController::kFwdSoftLimit : 0;
/* rev-soft-limit */
val = 0;
status = m_impl->GetStckyFault_RevSoftLim(val);
if (status != CTR_OKAY)
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
retval |= (val) ? CANSpeedController::kRevSoftLimit : 0;
return retval;
}
void CANTalon::ClearStickyFaults() {
CTR_Code status = m_impl->ClearStickyFaults();
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
/**
* Set the maximum voltage change rate. This ramp rate is in affect regardless
* of which control mode the TALON is in.
*
* When in PercentVbus or Voltage output mode, the rate at which the voltage
* changes can be limited to reduce current spikes. Set this to 0.0 to disable
* rate limiting.
*
* @param rampRate The maximum rate of voltage change in Percent Voltage mode
* in V/s.
*/
void CANTalon::SetVoltageRampRate(double rampRate) {
/* Caller is expressing ramp as Voltage per sec, assuming 12V is full.
Talon's throttle ramp is in dThrot/d10ms. 1023 is full fwd, -1023 is
full rev. */
double rampRatedThrotPer10ms = (rampRate * 1023.0 / 12.0) / 100;
CTR_Code status = m_impl->SetRampThrottle((int)rampRatedThrotPer10ms);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
void CANTalon::SetVoltageCompensationRampRate(double rampRate) {
/* when in voltage compensation mode, the voltage compensation rate
directly caps the change in target voltage */
CTR_Code status = CTR_OKAY;
status = m_impl->SetVoltageCompensationRate(rampRate / 1000);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* Sets a voltage change rate that applies only when a close loop contorl mode
* is enabled.
* This allows close loop specific ramp behavior.
*
* @param rampRate The maximum rate of voltage change in Percent Voltage mode
* in V/s.
*/
void CANTalon::SetCloseLoopRampRate(double rampRate) {
CTR_Code status = m_impl->SetCloseLoopRampRate(
m_profile, rampRate * 1023.0 / 12.0 / 1000.0);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* @return The version of the firmware running on the Talon
*/
uint32_t CANTalon::GetFirmwareVersion() const {
int firmwareVersion;
CTR_Code status = m_impl->RequestParam(CanTalonSRX::eFirmVers);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
std::this_thread::sleep_for(
std::chrono::microseconds(kDelayForSolicitedSignalsUs));
status =
m_impl->GetParamResponseInt32(CanTalonSRX::eFirmVers, firmwareVersion);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
/* only sent once on boot */
// CTR_Code status = m_impl->GetFirmVers(firmwareVersion);
// if (status != CTR_OKAY) {
// wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
//}
return firmwareVersion;
}
/**
* @return The accumulator for I gain.
*/
int CANTalon::GetIaccum() const {
CTR_Code status = m_impl->RequestParam(CanTalonSRX::ePidIaccum);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
// small yield for getting response
std::this_thread::sleep_for(
std::chrono::microseconds(kDelayForSolicitedSignalsUs));
int iaccum;
status = m_impl->GetParamResponseInt32(CanTalonSRX::ePidIaccum, iaccum);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return iaccum;
}
/**
* 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, HAL_GetErrorMessage(status));
}
}
/**
* 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, HAL_GetErrorMessage(status));
}
}
/**
* @return nonzero if brake is enabled during neutral. Zero if coast is enabled
* during neutral.
*/
int CANTalon::GetBrakeEnableDuringNeutral() const {
int brakeEn = 0;
CTR_Code status = m_impl->GetBrakeIsEnabled(brakeEn);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
return brakeEn;
}
/**
* Configure how many codes per revolution are generated by your encoder.
*
* @param codesPerRev The number of counts per revolution.
*/
void CANTalon::ConfigEncoderCodesPerRev(uint16_t codesPerRev) {
/* first save the scalar so that all getters/setter work as the user expects
*/
m_codesPerRev = codesPerRev;
/* next send the scalar to the Talon over CAN. This is so that the Talon can
* report it to whoever needs it, like the webdash. Don't bother checking
* the return, this is only for instrumentation and is not necessary for
* Talon functionality.
*/
(void)m_impl->SetParam(CanTalonSRX::eNumberEncoderCPR, m_codesPerRev);
}
/**
* Configure the number of turns on the potentiometer.
*
* @param turns The number of turns of the potentiometer.
*/
void CANTalon::ConfigPotentiometerTurns(uint16_t turns) {
/* first save the scalar so that all getters/setter work as the user expects
*/
m_numPotTurns = turns;
/* next send the scalar to the Talon over CAN. This is so that the Talon can
* report it to whoever needs it, like the webdash. Don't bother checking
* the return, this is only for instrumentation and is not necessary for
* Talon functionality.
*/
(void)m_impl->SetParam(CanTalonSRX::eNumberPotTurns, m_numPotTurns);
}
/**
* @deprecated not implemented
*/
void CANTalon::ConfigSoftPositionLimits(double forwardLimitPosition,
double reverseLimitPosition) {
ConfigLimitMode(kLimitMode_SoftPositionLimits);
ConfigForwardLimit(forwardLimitPosition);
ConfigReverseLimit(reverseLimitPosition);
}
/**
* TODO documentation (see CANJaguar.cpp)
*/
void CANTalon::DisableSoftPositionLimits() {
ConfigLimitMode(kLimitMode_SwitchInputsOnly);
}
/**
* Overrides the forward and reverse limit switch enables.
*
* Unlike ConfigLimitMode, this function allows individual control of forward
* and reverse limit switch enables.
* Unlike ConfigLimitMode, this function does not affect the soft-limit features
* of Talon SRX.
* @see ConfigLimitMode()
*/
void CANTalon::ConfigLimitSwitchOverrides(bool bForwardLimitSwitchEn,
bool bReverseLimitSwitchEn) {
CTR_Code status = CTR_OKAY;
int fwdRevEnable;
/* chose correct signal value based on caller's requests enables */
if (!bForwardLimitSwitchEn) {
/* caller wants Forward Limit Switch OFF */
if (!bReverseLimitSwitchEn) {
/* caller wants both OFF */
fwdRevEnable = CanTalonSRX::kLimitSwitchOverride_DisableFwd_DisableRev;
} else {
/* caller Forward OFF and Reverse ON */
fwdRevEnable = CanTalonSRX::kLimitSwitchOverride_DisableFwd_EnableRev;
}
} else {
/* caller wants Forward Limit Switch ON */
if (!bReverseLimitSwitchEn) {
/* caller wants Forward ON and Reverse OFF */
fwdRevEnable = CanTalonSRX::kLimitSwitchOverride_EnableFwd_DisableRev;
} else {
/* caller wants both ON */
fwdRevEnable = CanTalonSRX::kLimitSwitchOverride_EnableFwd_EnableRev;
}
}
/* update signal and error check code */
status = m_impl->SetOverrideLimitSwitchEn(fwdRevEnable);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* 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, HAL_GetErrorMessage(status));
}
status = m_impl->SetReverseSoftEnable(false);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(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, HAL_GetErrorMessage(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, HAL_GetErrorMessage(status));
}
status = m_impl->SetReverseSoftEnable(true);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(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, HAL_GetErrorMessage(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, HAL_GetErrorMessage(status));
}
status = m_impl->SetReverseSoftEnable(false);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(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, HAL_GetErrorMessage(status));
}
break;
}
}
/**
* TODO documentation (see CANJaguar.cpp)
*/
void CANTalon::ConfigForwardLimit(double forwardLimitPosition) {
CTR_Code status = CTR_OKAY;
int32_t nativeLimitPos =
ScaleRotationsToNativeUnits(m_feedbackDevice, forwardLimitPosition);
status = m_impl->SetForwardSoftLimit(nativeLimitPos);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* Set the Forward Soft Limit Enable.
*
* This is the same setting that is in the Web-Based Configuration.
*
* @param bForwardSoftLimitEn true to enable Soft limit, false to disable.
*/
void CANTalon::ConfigForwardSoftLimitEnable(bool bForwardSoftLimitEn) {
CTR_Code status = CTR_OKAY;
status = m_impl->SetForwardSoftEnable(bForwardSoftLimitEn);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* Set the Reverse Soft Limit Enable.
*
* This is the same setting that is in the Web-Based Configuration.
*
* @param bReverseSoftLimitEn true to enable Soft limit, false to disable.
*/
void CANTalon::ConfigReverseSoftLimitEnable(bool bReverseSoftLimitEn) {
CTR_Code status = CTR_OKAY;
status = m_impl->SetReverseSoftEnable(bReverseSoftLimitEn);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* Change the fwd limit switch setting to normally open or closed.
* Talon will disable momentarilly if the Talon's current setting
* is dissimilar to the caller's requested setting.
*
* Since Talon saves setting to flash this should only affect
* a given Talon initially during robot install.
*
* @param normallyOpen true for normally open. false for normally closed.
*/
void CANTalon::ConfigFwdLimitSwitchNormallyOpen(bool normallyOpen) {
CTR_Code status =
m_impl->SetParam(CanTalonSRX::eOnBoot_LimitSwitch_Forward_NormallyClosed,
normallyOpen ? 0 : 1);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* Change the rev limit switch setting to normally open or closed.
* Talon will disable momentarilly if the Talon's current setting
* is dissimilar to the caller's requested setting.
*
* Since Talon saves setting to flash this should only affect
* a given Talon initially during robot install.
*
* @param normallyOpen true for normally open. false for normally closed.
*/
void CANTalon::ConfigRevLimitSwitchNormallyOpen(bool normallyOpen) {
CTR_Code status =
m_impl->SetParam(CanTalonSRX::eOnBoot_LimitSwitch_Reverse_NormallyClosed,
normallyOpen ? 0 : 1);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* TODO documentation (see CANJaguar.cpp)
*/
void CANTalon::ConfigReverseLimit(double reverseLimitPosition) {
CTR_Code status = CTR_OKAY;
int32_t nativeLimitPos =
ScaleRotationsToNativeUnits(m_feedbackDevice, reverseLimitPosition);
status = m_impl->SetReverseSoftLimit(nativeLimitPos);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* TODO documentation (see CANJaguar.cpp)
*/
void CANTalon::ConfigMaxOutputVoltage(double voltage) {
/* config peak throttle when in closed-loop mode in the fwd and rev direction.
*/
ConfigPeakOutputVoltage(voltage, -voltage);
}
void CANTalon::ConfigPeakOutputVoltage(double forwardVoltage,
double reverseVoltage) {
/* bounds checking */
if (forwardVoltage > 12)
forwardVoltage = 12;
else if (forwardVoltage < 0)
forwardVoltage = 0;
if (reverseVoltage > 0)
reverseVoltage = 0;
else if (reverseVoltage < -12)
reverseVoltage = -12;
/* config calls */
ConfigSetParameter(CanTalonSRX::ePeakPosOutput, 1023 * forwardVoltage / 12.0);
ConfigSetParameter(CanTalonSRX::ePeakNegOutput, 1023 * reverseVoltage / 12.0);
}
void CANTalon::ConfigNominalOutputVoltage(double forwardVoltage,
double reverseVoltage) {
/* bounds checking */
if (forwardVoltage > 12)
forwardVoltage = 12;
else if (forwardVoltage < 0)
forwardVoltage = 0;
if (reverseVoltage > 0)
reverseVoltage = 0;
else if (reverseVoltage < -12)
reverseVoltage = -12;
/* config calls */
ConfigSetParameter(CanTalonSRX::eNominalPosOutput,
1023 * forwardVoltage / 12.0);
ConfigSetParameter(CanTalonSRX::eNominalNegOutput,
1023 * reverseVoltage / 12.0);
}
/**
* General set frame. Since the parameter is a general integral type, this can
* be used for testing future features.
*/
void CANTalon::ConfigSetParameter(uint32_t paramEnum, double value) {
CTR_Code status;
/* config peak throttle when in closed-loop mode in the positive direction. */
status = m_impl->SetParam((CanTalonSRX::param_t)paramEnum, value);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* General get frame. Since the parameter is a general integral type, this can
* be used for testing future features.
*/
bool CANTalon::GetParameter(uint32_t paramEnum, double& dvalue) const {
bool retval = true;
/* send the request frame */
CTR_Code status = m_impl->RequestParam((CanTalonSRX::param_t)paramEnum);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
retval = false;
}
/* small yield for getting response */
std::this_thread::sleep_for(
std::chrono::microseconds(kDelayForSolicitedSignalsUs));
/* get the last received update */
status = m_impl->GetParamResponse((CanTalonSRX::param_t)paramEnum, dvalue);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
retval = false;
}
return retval;
}
/**
* TODO documentation (see CANJaguar.cpp)
*/
void CANTalon::ConfigFaultTime(float faultTime) {
/* SRX does not have fault time. SRX motor drive is only disabled for soft
* limits and limit-switch faults. */
wpi_setWPIErrorWithContext(IncompatibleMode, "Fault Time not supported.");
}
/**
* Fixup the sendMode so Set() serializes the correct demand value.
* Also fills the modeSelecet in the control frame to disabled.
* @param mode Control mode to ultimately enter once user calls Set().
* @see Set()
*/
void CANTalon::ApplyControlMode(CANSpeedController::ControlMode mode) {
m_controlMode = mode;
HAL_Report(HALUsageReporting::kResourceType_CANTalonSRX, m_deviceNumber + 1,
mode);
switch (mode) {
case kPercentVbus:
m_sendMode = kThrottle;
break;
case kCurrent:
m_sendMode = kCurrentMode;
break;
case kSpeed:
m_sendMode = kSpeedMode;
break;
case kPosition:
m_sendMode = kPositionMode;
break;
case kVoltage:
m_sendMode = kVoltageMode;
break;
case kFollower:
m_sendMode = kFollowerMode;
break;
case kMotionProfile:
m_sendMode = kMotionProfileMode;
break;
}
// Keep the talon disabled until Set() is called.
CTR_Code status = m_impl->SetModeSelect((int)kDisabled);
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* TODO documentation (see CANJaguar.cpp)
*/
void CANTalon::SetControlMode(CANSpeedController::ControlMode mode) {
if (m_controlMode == mode) {
/* we already are in this mode, don't perform disable workaround */
} else {
ApplyControlMode(mode);
}
}
/**
* TODO documentation (see CANJaguar.cpp)
*/
CANSpeedController::ControlMode CANTalon::GetControlMode() const {
return m_controlMode;
}
void CANTalon::SetExpiration(float timeout) {
m_safetyHelper->SetExpiration(timeout);
}
float CANTalon::GetExpiration() const {
return m_safetyHelper->GetExpiration();
}
bool CANTalon::IsAlive() const { return m_safetyHelper->IsAlive(); }
bool CANTalon::IsSafetyEnabled() const {
return m_safetyHelper->IsSafetyEnabled();
}
void CANTalon::SetSafetyEnabled(bool enabled) {
m_safetyHelper->SetSafetyEnabled(enabled);
}
void CANTalon::GetDescription(std::ostringstream& desc) const {
desc << "CANTalon ID " << m_deviceNumber;
}
/**
* @param devToLookup FeedbackDevice to lookup the scalar for. Because Talon
* allows multiple sensors to be attached simultaneously,
* caller must specify which sensor to lookup.
* @return The number of native Talon units per rotation of the selected
* sensor. Zero if the necessary sensor information is not available.
* @see ConfigEncoderCodesPerRev
* @see ConfigPotentiometerTurns
*/
double CANTalon::GetNativeUnitsPerRotationScalar(
FeedbackDevice devToLookup) const {
bool scalingAvail = false;
CTR_Code status = CTR_OKAY;
double retval = 0;
switch (devToLookup) {
case QuadEncoder: { /* When caller wants to lookup Quadrature, the QEI may
* be in 1x if the selected feedback is edge counter.
* Additionally if the quadrature source is the CTRE Mag
* encoder, then the CPR is known.
* This is nice in that the calling app does not require
* knowing the CPR at all.
* So do both checks here.
*/
int32_t qeiPulsePerCount = 4; /* default to 4x */
switch (m_feedbackDevice) {
case CtreMagEncoder_Relative:
case CtreMagEncoder_Absolute:
/* we assume the quadrature signal comes from the MagEnc,
of which we know the CPR already */
retval = kNativePwdUnitsPerRotation;
scalingAvail = true;
break;
case EncRising: /* Talon's QEI is setup for 1x, so perform 1x math */
case EncFalling:
qeiPulsePerCount = 1;
break;
case QuadEncoder: /* Talon's QEI is 4x */
default: /* pulse width and everything else, assume its regular quad
use. */
break;
}
if (scalingAvail) {
/* already deduced the scalar above, we're done. */
} else {
/* we couldn't deduce the scalar just based on the selection */
if (0 == m_codesPerRev) {
/* caller has never set the CPR. Most likely caller
* is just using engineering units so fall to the
* bottom of this func.
*/
} else {
/* Talon expects PPR units */
retval = qeiPulsePerCount * m_codesPerRev;
scalingAvail = true;
}
}
} break;
case EncRising:
case EncFalling:
if (0 == m_codesPerRev) {
/* caller has never set the CPR. Most likely caller
* is just using engineering units so fall to the
* bottom of this func.
*/
} else {
/* Talon expects PPR units */
retval = 1 * m_codesPerRev;
scalingAvail = true;
}
break;
case AnalogPot:
case AnalogEncoder:
if (0 == m_numPotTurns) {
/* caller has never set the CPR. Most likely caller
* is just using engineering units so fall to the
* bottom of this func.
*/
} else {
retval = (double)kNativeAdcUnitsPerRotation / m_numPotTurns;
scalingAvail = true;
}
break;
case CtreMagEncoder_Relative:
case CtreMagEncoder_Absolute:
case PulseWidth:
retval = kNativePwdUnitsPerRotation;
scalingAvail = true;
break;
}
/* handle any detected errors */
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
/* if scaling information is not possible, signal caller
* by returning zero
*/
if (false == scalingAvail) retval = 0;
return retval;
}
/**
* @param fullRotations double precision value representing number of
* rotations of selected feedback sensor. If user has
* never called the config routine for the selected
* sensor, then the caller is likely passing rotations
* in engineering units already, in which case it is
* returned as is.
* @see ConfigPotentiometerTurns
* @see ConfigEncoderCodesPerRev
* @return fullRotations in native engineering units of the Talon SRX firmware.
*/
int32_t CANTalon::ScaleRotationsToNativeUnits(FeedbackDevice devToLookup,
double fullRotations) const {
/* first assume we don't have config info, prep the default return */
int32_t retval = (int32_t)fullRotations;
/* retrieve scaling info */
double scalar = GetNativeUnitsPerRotationScalar(devToLookup);
/* apply scalar if its available */
if (scalar > 0) retval = (int32_t)(fullRotations * scalar);
return retval;
}
/**
* @param rpm double precision value representing number of rotations per
* minute of selected feedback sensor. If user has never called
* the config routine for the selected sensor, then the caller is
* likely passing rotations in engineering units already, in which
* case it is returned as is.
* @see ConfigPotentiometerTurns
* @see ConfigEncoderCodesPerRev
* @return sensor velocity in native engineering units of the Talon SRX
* firmware.
*/
int32_t CANTalon::ScaleVelocityToNativeUnits(FeedbackDevice devToLookup,
double rpm) const {
/* first assume we don't have config info, prep the default return */
int32_t retval = (int32_t)rpm;
/* retrieve scaling info */
double scalar = GetNativeUnitsPerRotationScalar(devToLookup);
/* apply scalar if its available */
if (scalar > 0) retval = (int32_t)(rpm * kMinutesPer100msUnit * scalar);
return retval;
}
/**
* @param nativePos integral position of the feedback sensor in native Talon
* SRX units. If user has never called the config routine for
* the selected sensor, then the return will be in TALON SRX
* units as well to match the behavior in the 2015 season.
* @see ConfigPotentiometerTurns
* @see ConfigEncoderCodesPerRev
* @return double precision number of rotations, unless config was never
* performed.
*/
double CANTalon::ScaleNativeUnitsToRotations(FeedbackDevice devToLookup,
int32_t nativePos) const {
/* first assume we don't have config info, prep the default return */
double retval = (double)nativePos;
/* retrieve scaling info */
double scalar = GetNativeUnitsPerRotationScalar(devToLookup);
/* apply scalar if its available */
if (scalar > 0) retval = ((double)nativePos) / scalar;
return retval;
}
/**
* @param nativeVel integral velocity of the feedback sensor in native Talon
* SRX units. If user has never called the config routine for
* the selected sensor, then the return will be in TALON SRX
* units as well to match the behavior in the 2015 season.
* @see ConfigPotentiometerTurns
* @see ConfigEncoderCodesPerRev
* @return double precision of sensor velocity in RPM, unless config was never
* performed.
*/
double CANTalon::ScaleNativeUnitsToRpm(FeedbackDevice devToLookup,
int32_t nativeVel) const {
/* first assume we don't have config info, prep the default return */
double retval = (double)nativeVel;
/* retrieve scaling info */
double scalar = GetNativeUnitsPerRotationScalar(devToLookup);
/* apply scalar if its available */
if (scalar > 0)
retval = (double)(nativeVel) / (scalar * kMinutesPer100msUnit);
return retval;
}
/**
* Enables Talon SRX to automatically zero the Sensor Position whenever an
* edge is detected on the index signal.
*
* @param enable boolean input, pass true to enable feature or false to
* disable.
* @param risingEdge boolean input, pass true to clear the position on rising
* edge, pass false to clear the position on falling edge.
*/
void CANTalon::EnableZeroSensorPositionOnIndex(bool enable, bool risingEdge) {
if (enable) {
/* enable the feature, update the edge polarity first to ensure
it is correct before the feature is enabled. */
ConfigSetParameter(CanTalonSRX::eQuadIdxPolarity, risingEdge ? 1 : 0);
ConfigSetParameter(CanTalonSRX::eClearPositionOnIdx, 1);
} else {
/* disable the feature first, then update the edge polarity. */
ConfigSetParameter(CanTalonSRX::eClearPositionOnIdx, 0);
ConfigSetParameter(CanTalonSRX::eQuadIdxPolarity, risingEdge ? 1 : 0);
}
}
/**
* Calling application can opt to speed up the handshaking between the robot API
* and the Talon to increase the download rate of the Talon's Motion Profile.
* Ideally the period should be no more than half the period of a trajectory
* point.
*/
void CANTalon::ChangeMotionControlFramePeriod(int periodMs) {
m_impl->ChangeMotionControlFramePeriod(periodMs);
}
/**
* Clear the buffered motion profile in both Talon RAM (bottom), and in the API
* (top).
*
* Be sure to check GetMotionProfileStatus() to know when the buffer is actually
* cleared.
*/
void CANTalon::ClearMotionProfileTrajectories() {
m_impl->ClearMotionProfileTrajectories();
}
/**
* Retrieve just the buffer count for the api-level (top) buffer.
*
* This routine performs no CAN or data structure lookups, so its fast and ideal
* if caller needs to quickly poll the progress of trajectory points being
* emptied into Talon's RAM. Otherwise just use GetMotionProfileStatus.
*
* @return number of trajectory points in the top buffer.
*/
int CANTalon::GetMotionProfileTopLevelBufferCount() {
return m_impl->GetMotionProfileTopLevelBufferCount();
}
/**
* Push another trajectory point into the top level buffer (which is emptied
* into the Talon's bottom buffer as room allows).
*
* @param trajPt the trajectory point to insert into buffer.
* @return true if trajectory point push ok. CTR_BufferFull if buffer is full
* due to kMotionProfileTopBufferCapacity.
*/
bool CANTalon::PushMotionProfileTrajectory(const TrajectoryPoint& trajPt) {
/* convert positiona and velocity to native units */
int32_t targPos =
ScaleRotationsToNativeUnits(m_feedbackDevice, trajPt.position);
int32_t targVel =
ScaleVelocityToNativeUnits(m_feedbackDevice, trajPt.velocity);
/* bounds check signals that require it */
uint32_t profileSlotSelect = (trajPt.profileSlotSelect) ? 1 : 0;
uint8_t timeDurMs = (trajPt.timeDurMs >= 255)
? 255
: trajPt.timeDurMs; /* cap time to 255ms */
/* send it to the top level buffer */
CTR_Code status = m_impl->PushMotionProfileTrajectory(
targPos, targVel, profileSlotSelect, timeDurMs, trajPt.velocityOnly,
trajPt.isLastPoint, trajPt.zeroPos);
return (status == CTR_OKAY) ? true : false;
}
/**
* @return true if api-level (top) buffer is full.
*/
bool CANTalon::IsMotionProfileTopLevelBufferFull() {
return m_impl->IsMotionProfileTopLevelBufferFull();
}
/**
* This must be called periodically to funnel the trajectory points from the
* API's top level buffer to the Talon's bottom level buffer. Recommendation
* is to call this twice as fast as the executation rate of the motion profile.
* So if MP is running with 20ms trajectory points, try calling this routine
* every 10ms. All motion profile functions are thread-safe through the use of
* a mutex, so there is no harm in having the caller utilize threading.
*/
void CANTalon::ProcessMotionProfileBuffer() {
m_impl->ProcessMotionProfileBuffer();
}
/**
* Retrieve all status information.
*
* Since this all comes from one CAN frame, its ideal to have one routine to
* retrieve the frame once and decode everything.
*
* @param [out] motionProfileStatus contains all progress information on the
* currently running MP.
*/
void CANTalon::GetMotionProfileStatus(
MotionProfileStatus& motionProfileStatus) {
uint32_t flags;
uint32_t profileSlotSelect;
int32_t targPos, targVel;
uint32_t topBufferRem, topBufferCnt, btmBufferCnt;
uint32_t outputEnable;
/* retrieve all motion profile signals from status frame */
CTR_Code status = m_impl->GetMotionProfileStatus(
flags, profileSlotSelect, targPos, targVel, topBufferRem, topBufferCnt,
btmBufferCnt, outputEnable);
/* completely update the caller's structure */
motionProfileStatus.topBufferRem = topBufferRem;
motionProfileStatus.topBufferCnt = topBufferCnt;
motionProfileStatus.btmBufferCnt = btmBufferCnt;
motionProfileStatus.hasUnderrun =
(flags & CanTalonSRX::kMotionProfileFlag_HasUnderrun) ? true : false;
motionProfileStatus.isUnderrun =
(flags & CanTalonSRX::kMotionProfileFlag_IsUnderrun) ? true : false;
motionProfileStatus.activePointValid =
(flags & CanTalonSRX::kMotionProfileFlag_ActTraj_IsValid) ? true : false;
motionProfileStatus.activePoint.isLastPoint =
(flags & CanTalonSRX::kMotionProfileFlag_ActTraj_IsLast) ? true : false;
motionProfileStatus.activePoint.velocityOnly =
(flags & CanTalonSRX::kMotionProfileFlag_ActTraj_VelOnly) ? true : false;
motionProfileStatus.activePoint.position =
ScaleNativeUnitsToRotations(m_feedbackDevice, targPos);
motionProfileStatus.activePoint.velocity =
ScaleNativeUnitsToRpm(m_feedbackDevice, targVel);
motionProfileStatus.activePoint.profileSlotSelect = profileSlotSelect;
switch (outputEnable) {
case CanTalonSRX::kMotionProf_Disabled:
motionProfileStatus.outputEnable = SetValueMotionProfileDisable;
break;
case CanTalonSRX::kMotionProf_Enable:
motionProfileStatus.outputEnable = SetValueMotionProfileEnable;
break;
case CanTalonSRX::kMotionProf_Hold:
motionProfileStatus.outputEnable = SetValueMotionProfileHold;
break;
default:
motionProfileStatus.outputEnable = SetValueMotionProfileDisable;
break;
}
motionProfileStatus.activePoint.zeroPos =
false; /* this signal is only used sending pts to Talon */
motionProfileStatus.activePoint.timeDurMs =
0; /* this signal is only used sending pts to Talon */
if (status != CTR_OKAY) {
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
}
/**
* Clear the hasUnderrun flag in Talon's Motion Profile Executer when MPE is
* ready for another point, but the low level buffer is empty.
*
* Once the Motion Profile Executer sets the hasUnderrun flag, it stays set
* until Robot Application clears it with this routine, which ensures Robot
* Application gets a chance to instrument or react. Caller could also check
* the isUnderrun flag which automatically clears when fault condition is
* removed.
*/
void CANTalon::ClearMotionProfileHasUnderrun() {
ConfigSetParameter(CanTalonSRX::eMotionProfileHasUnderrunErr, 0);
}
/**
* Common interface for inverting direction of a speed controller.
*
* Only works in PercentVbus, speed, and Voltage modes.
*
* @param isInverted The state of inversion, true is inverted.
*/
void CANTalon::SetInverted(bool isInverted) { m_isInverted = isInverted; }
/**
* Common interface for the inverting direction of a speed controller.
*
* @return isInverted The state of inversion, true is inverted.
*/
bool CANTalon::GetInverted() const { return m_isInverted; }
/**
* Common interface for stopping the motor until the next Set() call.
*
* Part of the MotorSafety interface.
*
* @deprecated Call Disable instead.
*/
void CANTalon::StopMotor() {
Disable();
m_stopped = true;
}
void CANTalon::ValueChanged(ITable* source, llvm::StringRef key,
std::shared_ptr<nt::Value> value, bool isNew) {
if (key == "Mode" && value->IsDouble())
SetControlMode(
static_cast<CANSpeedController::ControlMode>(value->GetDouble()));
if (key == "p" && value->IsDouble()) SetP(value->GetDouble());
if (key == "i" && value->IsDouble()) SetI(value->GetDouble());
if (key == "d" && value->IsDouble()) SetD(value->GetDouble());
if (key == "f" && value->IsDouble()) SetF(value->GetDouble());
if (key == "Enabled" && value->IsBoolean()) {
if (value->GetBoolean()) {
Enable();
} else {
Disable();
}
}
if (key == "Value" && value->IsDouble()) Set(value->GetDouble());
}
bool CANTalon::IsModePID(CANSpeedController::ControlMode mode) const {
return mode == kCurrent || mode == kSpeed || mode == kPosition;
}
void CANTalon::UpdateTable() {
if (m_table != nullptr) {
m_table->PutString("~TYPE~", "CANSpeedController");
m_table->PutString("Type", "CANTalon");
m_table->PutNumber("Mode", m_controlMode);
m_table->PutNumber("p", GetP());
m_table->PutNumber("i", GetI());
m_table->PutNumber("d", GetD());
m_table->PutNumber("f", GetF());
m_table->PutBoolean("Enabled", IsControlEnabled());
m_table->PutNumber("Value", Get());
}
}
void CANTalon::StartLiveWindowMode() {
if (m_table != nullptr) {
m_table->AddTableListener(this, true);
}
}
void CANTalon::StopLiveWindowMode() {
if (m_table != nullptr) {
m_table->RemoveTableListener(this);
}
}
std::string CANTalon::GetSmartDashboardType() const {
return "CANSpeedController";
}
void CANTalon::InitTable(std::shared_ptr<ITable> subTable) {
m_table = subTable;
UpdateTable();
}
std::shared_ptr<ITable> CANTalon::GetTable() const { return m_table; }
Reference in New Issue View Git Blame Copy Permalink