public static string ToString(FeedbackDevice value)
{
switch (value)
{
case FeedbackDevice.QuadEncoder: return("FeedbackDevice.QuadEncoder");
case FeedbackDevice.Analog: return("FeedbackDevice.Analog");
case FeedbackDevice.Tachometer: return("FeedbackDevice.Tachometer");
case FeedbackDevice.PulseWidthEncodedPosition: return("FeedbackDevice.PulseWidthEncodedPosition");
case FeedbackDevice.SensorSum: return("FeedbackDevice.SensorSum");
case FeedbackDevice.SensorDifference: return("FeedbackDevice.SensorDifference");
case FeedbackDevice.RemoteSensor0: return("FeedbackDevice.RemoteSensor0");
case FeedbackDevice.RemoteSensor1: return("FeedbackDevice.RemoteSensor1");
case FeedbackDevice.SoftwareEmulatedSensor: return("FeedbackDevice.SoftwareEmulatedSensor");
default: return("InvalidValue");
}
}
public TalonSRXConfiguration()
{
primaryPID = new TalonSRXPIDSetConfiguration();
auxilaryPID = new TalonSRXPIDSetConfiguration();
forwardLimitSwitchSource = LimitSwitchSource.FeedbackConnector;
reverseLimitSwitchSource = LimitSwitchSource.FeedbackConnector;
sum_0 = FeedbackDevice.QuadEncoder;
sum_1 = FeedbackDevice.QuadEncoder;
diff_0 = FeedbackDevice.QuadEncoder;
diff_1 = FeedbackDevice.QuadEncoder;
peakCurrentLimit = 1;
peakCurrentDuration = 1;
continuousCurrentLimit = 1;
}
internal double ScaleNativeUnitsToRpm(FeedbackDevice devToLookup, long nativeVel)
{
double retVal = nativeVel;
double scalar = GetNativeUnitsPerRotationScalar(devToLookup);
if (scalar > 0)
{
retVal = nativeVel / (scalar * MinutesPer100MsUnits);
}
return retVal;
}
internal double ScaleNativeUnitsToRotations(FeedbackDevice devToLookup, int nativePos)
{
double retVal = nativePos;
double scalar = GetNativeUnitsPerRotationScalar(devToLookup);
if (scalar > 0)
{
retVal = nativePos / scalar;
}
return retVal;
}
internal int ScaleVelocityToNativeUnits(FeedbackDevice devToLookup, double rpm)
{
int retVal = (int)rpm;
double scalar = GetNativeUnitsPerRotationScalar(devToLookup);
if (scalar > 0)
{
retVal = (int)(rpm * MinutesPer100MsUnits * scalar);
}
return retVal;
}
public SensoredGearbox(float unitsPerRevolution, IMotorControllerEnhanced mc0, IFollower mc1, IFollower mc2, FeedbackDevice feedbackDevice)
: this(unitsPerRevolution, mc0, new IFollower[] { mc1, mc2 }, feedbackDevice)
{
}
public ErrorCode ConfigSelectedFeedbackSensor(FeedbackDevice feedbackDevice, int pidIdx, int timeoutMs = 0)
{
return(_ll.ConfigSelectedFeedbackSensor(feedbackDevice, pidIdx, timeoutMs));
}
public void SetFeedbackDevice(FeedbackDevice device)
{
FeedBackDevice = device;
}
/**
* @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 float precision number of rotations, unless config was never performed.
*/
private float ScaleNativeUnitsToRotations(FeedbackDevice devToLookup, Int32 nativePos)
{
/* first assume we don't have config info, prep the default return */
float retval = (float)nativePos;
/* retrieve scaling info */
float scalar = GetNativeUnitsPerRotationScalar(devToLookup);
/* apply scalar if its available */
if (scalar > 0)
retval = ((float)nativePos) / scalar;
return retval;
}
/**
* @param rpm float 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.
*/
private Int32 ScaleVelocityToNativeUnits(FeedbackDevice devToLookup, float rpm)
{
/* first assume we don't have config info, prep the default return */
Int32 retval = (Int32)rpm;
/* retrieve scaling info */
float scalar = GetNativeUnitsPerRotationScalar(devToLookup);
/* apply scalar if its available */
if (scalar > 0)
retval = (Int32)(rpm * kMinutesPer100msUnit * scalar);
return retval;
}
/**
* @param fullRotations float 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.
*/
private Int32 ScaleRotationsToNativeUnits(FeedbackDevice devToLookup, float fullRotations)
{
/* first assume we don't have config info, prep the default return */
Int32 retval = (Int32)fullRotations;
/* retrieve scaling info */
float scalar = GetNativeUnitsPerRotationScalar(devToLookup);
/* apply scalar if its available */
if (scalar > 0)
retval = (Int32)(fullRotations * scalar);
return retval;
}
/**
* @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
*/
private float GetNativeUnitsPerRotationScalar(FeedbackDevice devToLookup)
{
bool scalingAvail = false;
int status = CAN_OK;
float retval = 0;
switch (devToLookup)
{
case FeedbackDevice.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 qeiPulsePerCount = 4; /* default to 4x */
switch (m_feedbackDevice)
{
case FeedbackDevice.CtreMagEncoder_Relative:
case FeedbackDevice.CtreMagEncoder_Absolute:
/* we assume the quadrature signal comes from the MagEnc,
of which we know the CPR already */
retval = kNativePwdUnitsPerRotation;
scalingAvail = true;
break;
case FeedbackDevice.EncRising: /* Talon's QEI is setup for 1x, so perform 1x math */
case FeedbackDevice.EncFalling:
qeiPulsePerCount = 1;
break;
case FeedbackDevice.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 FeedbackDevice.EncRising:
case FeedbackDevice.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 FeedbackDevice.AnalogPot:
case FeedbackDevice.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 = (float)kNativeAdcUnitsPerRotation / m_numPotTurns;
scalingAvail = true;
}
break;
case FeedbackDevice.CtreMagEncoder_Relative:
case FeedbackDevice.CtreMagEncoder_Absolute:
case FeedbackDevice.PulseWidth:
retval = kNativePwdUnitsPerRotation;
scalingAvail = true;
break;
}
/* handle any detected errors */
HandleStatus(status);
/* if scaling information is not possible, signal caller
by returning zero */
if (false == scalingAvail)
retval = 0;
return retval;
}
/**
* @param which feedback sensor to check it if is connected.
* @return status of caller's specified sensor type.
*/
public FeedbackDeviceStatus IsSensorPresent(FeedbackDevice feedbackDevice)
{
FeedbackDeviceStatus retval = FeedbackDeviceStatus.FeedbackStatusUnknown;
int param;
/* detecting sensor health depends on which sensor caller cares about */
switch (feedbackDevice)
{
case FeedbackDevice.QuadEncoder:
case FeedbackDevice.AnalogPot:
case FeedbackDevice.AnalogEncoder:
case FeedbackDevice.EncRising:
case FeedbackDevice.EncFalling:
/* no real good way to tell if these sensor
are actually present so return status unknown. */
break;
case FeedbackDevice.PulseWidth:
case FeedbackDevice.CtreMagEncoder_Relative:
case FeedbackDevice.CtreMagEncoder_Absolute:
/* all of these require pulse width signal to be present. */
int status = m_impl.IsPulseWidthSensorPresent(out param);
if (status != CAN_OK)
{
/* we're not getting status info, signal unknown status */
}
else {
/* param is updated */
if (param != 0)
{
/* pulse signal is present, sensor must be working since it always
generates a pulse waveform.*/
retval = FeedbackDeviceStatus.FeedbackStatusPresent;
}
else {
/* no pulse present, sensor disconnected */
retval = FeedbackDeviceStatus.FeedbackStatusNotPresent;
}
}
break;
}
return retval;
}
public CurrentLimitedSensorGearbox(float unitsPerRevolution, IMotorControllerEnhanced mc0, IMotorControllerEnhanced mc1, IMotorControllerEnhanced mc2, FeedbackDevice feedbackDevice)
: base(unitsPerRevolution, mc0, mc1, mc2, feedbackDevice)
{
}
public CurrentLimitedSensorGearbox(float unitsPerRevolution, IMotorControllerEnhanced master, IMotorControllerEnhanced[] followers, FeedbackDevice feedbackDevice) : base(unitsPerRevolution, master, followers, feedbackDevice)
{
}
/// <summary>
/// Constructs a CANTalon object.
/// </summary>
/// <param name="deviceNumber">The id of the Talon SRX this object will communicate with.</param>
/// <param name="controlPeriodMs">The update period to the Talon SRX. Defaults to 10ms.</param>
/// <param name="enablePeriodMs">The period in ms to send the enable control frame.</param>
public CANTalon(int deviceNumber, int controlPeriodMs, int enablePeriodMs)
{
if (deviceNumber < 0 || deviceNumber >= MaxTalonId)
{
throw new ArgumentOutOfRangeException(nameof(deviceNumber), "Talon IDs must be between 0 and 62 inclusive.");
}
TalonIds.Allocate(deviceNumber, $"CAN TalonSRX ID {deviceNumber} is already allocated.");
DeviceId = deviceNumber;
m_talonPointer = C_TalonSRX_Create3(deviceNumber, controlPeriodMs, enablePeriodMs);
m_safetyHelper = new MotorSafetyHelper(this);
m_controlEnabled = true;
m_setPoint = 0;
Profile = 0;
m_codesPerRev = 0;
m_numPotTurns = 0;
m_feedbackDevice = FeedbackDevice.QuadEncoder;
ApplyControlMode(ControlMode.PercentVbus);
LiveWindow.LiveWindow.AddActuator("CANTalonSRX", deviceNumber, this);
}
/// <summary>
/// Gets whether the sensor is present.
/// </summary>
/// <param name="feedbackDevice">The sensor to check for.</param>
/// <returns>The status of the feedback device.</returns>
public FeedbackDeviceStatus IsSensorPresent(FeedbackDevice feedbackDevice)
{
FeedbackDeviceStatus retVal = FeedbackDeviceStatus.FeedbackStatusUnknown;
switch (feedbackDevice)
{
case FeedbackDevice.QuadEncoder:
case FeedbackDevice.AnalogPotentiometer:
case FeedbackDevice.AnalogEncoder:
case FeedbackDevice.EncoderRising:
case FeedbackDevice.EncoderFalling:
break;
case FeedbackDevice.CtreMagEncoderRelative:
case FeedbackDevice.CtreMagEncoderAbsolute:
case FeedbackDevice.PulseWidth:
int value = 0;
CTR_Code status = C_TalonSRX_IsPulseWidthSensorPresent(m_talonPointer, ref value);
CheckCTRStatus(status);
if (value == 0)
{
retVal = FeedbackDeviceStatus.FeedbackStatusNotPresent;
}
else
{
retVal = FeedbackDeviceStatus.FeedbackStatusPresent;
}
break;
}
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 float precision of sensor velocity in RPM, unless config was never performed.
*/
private float ScaleNativeUnitsToRpm(FeedbackDevice devToLookup, Int32 nativeVel)
{
/* first assume we don't have config info, prep the default return */
float retval = (float)nativeVel;
/* retrieve scaling info */
float scalar = GetNativeUnitsPerRotationScalar(devToLookup);
/* apply scalar if its available */
if (scalar > 0)
retval = (float)(nativeVel) / (scalar * kMinutesPer100msUnit);
return retval;
}
/**
* Select the feedback device to use in closed-loop
*/
public void 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 */
int status = m_impl.SetFeedbackDeviceSelect((int)feedbackDevice);
HandleStatus(status);
}
internal double GetNativeUnitsPerRotationScalar(FeedbackDevice devToLookup)
{
double retVal = 0;
bool scalingAvail = false;
switch (devToLookup)
{
case FeedbackDevice.QuadEncoder:
int qeiPulsePerCount = 4;
switch (m_feedbackDevice)
{
case FeedbackDevice.CtreMagEncoderRelative:
case FeedbackDevice.CtreMagEncoderAbsolute:
retVal = NativePwdUnitsPerRotation;
scalingAvail = true;
break;
case FeedbackDevice.EncoderRising:
case FeedbackDevice.EncoderFalling:
qeiPulsePerCount = 1;
break;
}
if (scalingAvail)
{
}
else
{
if (0 == m_codesPerRev)
{
}
else
{
retVal = 4 * m_codesPerRev;
scalingAvail = true;
}
}
break;
case FeedbackDevice.AnalogPotentiometer:
case FeedbackDevice.AnalogEncoder:
if (0 == m_numPotTurns)
{
}
else
{
retVal = (double)NativeAdcUnitsPerRotation / m_numPotTurns;
scalingAvail = true;
}
break;
case FeedbackDevice.EncoderRising:
case FeedbackDevice.EncoderFalling:
if (0 == m_codesPerRev)
{
}
else
{
retVal = 1 * m_codesPerRev;
scalingAvail = true;
}
break;
case FeedbackDevice.CtreMagEncoderRelative:
case FeedbackDevice.CtreMagEncoderAbsolute:
case FeedbackDevice.PulseWidth:
retVal = NativePwdUnitsPerRotation;
scalingAvail = true;
break;
}
return !scalingAvail ? 0 : retVal;
}
/**
* Select what sensor term should be bound to switch feedback device.
* Sensor Sum = Sensor Sum Term 0 - Sensor Sum Term 1
* Sensor Difference = Sensor Diff Term 0 - Sensor Diff Term 1
* The four terms are specified with this routine. Then Sensor Sum/Difference
* can be selected for closed-looping.
*
* @param sensorTerm Which sensor term to bind to a feedback source.
* @param feedbackDevice The sensor signal to attach to sensorTerm.
* @param timeoutMs
* Timeout value in ms. If nonzero, function will wait for
* config success and report an error if it times out.
* If zero, no blocking or checking is performed.
* @return Error Code generated by function. 0 indicates no error.
*/
public ErrorCode ConfigSensorTerm(SensorTerm sensorTerm, FeedbackDevice feedbackDevice, int timeoutMs)
{
return(_ll.ConfigSensorTerm(sensorTerm, feedbackDevice, timeoutMs));
}
internal int ScaleRotationsToNativeUnits(FeedbackDevice devToLookup, double fullRotations)
{
int retVal = (int)fullRotations;
double scalar = GetNativeUnitsPerRotationScalar(devToLookup);
if (scalar > 0)
{
retVal = (int)(fullRotations * scalar);
}
return retVal;
}
public TalonSRXPIDSetConfiguration()
{
selectedFeedbackSensor = FeedbackDevice.QuadEncoder;
}
public SensoredGearbox(float unitsPerRevolution, IMotorControllerEnhanced master, IFollower[] followers, FeedbackDevice feedbackDevice) : base(unitsPerRevolution, master, followers)
{
_master = master;
_master.ConfigSelectedFeedbackSensor(feedbackDevice, 0);
}
