private void InitRelay()
{
CheckRelayChannel(m_channel);
int status = 0;
int portHandle = HAL_GetPort(m_channel);
if (m_direction == Direction.Both || m_direction == Direction.Forward)
{
m_forwardHandle = HAL_InitializeRelayPort(portHandle, true, ref status);
CheckStatusRange(status, 0, HAL_GetNumRelayHeaders(), m_channel);
Report(ResourceType.kResourceType_Relay, m_channel);
}
if (m_direction == Direction.Both || m_direction == Direction.Reverse)
{
m_reverseHandle = HAL_InitializeRelayPort(portHandle, false, ref status);
CheckStatusRange(status, 0, HAL_GetNumRelayHeaders(), m_channel);
Report(ResourceType.kResourceType_Relay, m_channel + 128);
}
m_safetyHelper = new MotorSafetyHelper(this);
m_safetyHelper.SafetyEnabled = false;
LiveWindow.LiveWindow.AddActuator("Relay", m_channel, this);
}
private void InitSafePWM()
{
m_safetyHelper = new MotorSafetyHelper(this)
{
Expiration = 0.0,
SafetyEnabled = false
};
}
private void SetupMotorSafety()
{
SafetyHelper = new MotorSafetyHelper(this)
{
Expiration = DefaultExpirationTime,
SafetyEnabled = true
};
}
private void InitSafePWM()
{
m_safetyHelper = new MotorSafetyHelper(this)
{
Expiration = 0.0,
SafetyEnabled = false
};
}
/// <summary>
/// Check the motors to see if any have timed out. </summary><remarks>
/// This static method is called periodically to poll all motors and stop any that have timed out.
/// </remarks>
public static void CheckMotors()
{
lock (s_lockObject)
{
for (MotorSafetyHelper msh = s_headHelper; msh != null; msh = msh.m_nextHelper)
{
msh.Check();
}
}
}
/// <summary>
/// The constructor for a <see cref="MotorSafetyHelper"/> object
/// </summary><remarks>
/// The helper object is constructed for every object that wants to implement the Motor
/// Safety protocol. The helper object has the code to actually do the timing and call the
/// motors Stop() method when the timeout expires. The motor object is expected to call the
/// Feed() method whenever the motors value is updated.
/// </remarks>
/// <param name="safeObject">A pointer to the motor object implementing <see cref="IMotorSafety"/>. This is used
/// to call the Stop() method on the motor</param>
public MotorSafetyHelper(IMotorSafety safeObject)
{
m_safeObject = safeObject;
SafetyEnabled = false;
Expiration = DefaultSafetyExpiration;
m_stopTime = GetFPGATimestamp();
lock (s_lockObject)
{
m_nextHelper = s_headHelper;
s_headHelper = this;
}
}
/// <summary>
/// The constructor for a <see cref="MotorSafetyHelper"/> object
/// </summary><remarks>
/// The helper object is constructed for every object that wants to implement the Motor
/// Safety protocol. The helper object has the code to actually do the timing and call the
/// motors Stop() method when the timeout expires. The motor object is expected to call the
/// Feed() method whenever the motors value is updated.
/// </remarks>
/// <param name="safeObject">A pointer to the motor object implementing <see cref="IMotorSafety"/>. This is used
/// to call the Stop() method on the motor</param>
public MotorSafetyHelper(IMotorSafety safeObject)
{
m_safeObject = safeObject;
SafetyEnabled = false;
Expiration = DefaultSafetyExpiration;
m_stopTime = GetFPGATimestamp();
lock (s_lockObject)
{
m_nextHelper = s_headHelper;
s_headHelper = this;
}
}
public CANTalon(int deviceNumber, int controlPeriodMs = 10)
{
DeviceID = deviceNumber;
m_impl = C_TalonSRX_Create(deviceNumber, controlPeriodMs);
m_safetyHelper = new MotorSafetyHelper(this);
m_controlEnabled = true;
m_setPoint = 0;
Profile = 0;
ApplyControlMode(ControlMode.PercentVbus);
LiveWindow.AddActuator("CANTalonSRX", deviceNumber, this);
HAL.Report(ResourceType.kResourceType_CANTalonSRX, (byte)(deviceNumber + 1), (byte)m_controlMode);
}
//The DS loop thread
private void Task()
{
m_isRunning = true;
//The safety counter is used in order to implement motor safety
int safetyCounter = 0;
while (m_isRunning)
{
//Wait for new DS data, grab the newest data, and return the semaphore.
HAL_WaitForDSData();
GetData();
// notify IsNewControlData variables
Interlocked.Exchange(ref m_newControlData, 1);
// notify WaitForData block
lock (m_waitForDataMutex)
{
m_waitForDataPredicate = true;
Monitor.PulseAll(m_waitForDataMutex);
}
//Every 4 loops (80ms) check all of the motors to make sure they have been updated
if (++safetyCounter >= 4)
{
MotorSafetyHelper.CheckMotors();
safetyCounter = 0;
}
//Report our program state.
if (m_userInDisabled)
{
HAL_ObserveUserProgramDisabled();
}
if (m_userInAutonomous)
{
HAL_ObserveUserProgramAutonomous();
}
if (m_userInTeleop)
{
HAL_ObserveUserProgramAutonomous();
}
if (m_userInTest)
{
HAL_ObserveUserProgramTest();
}
}
}
private void InitRelay()
{
CheckRelayChannel(m_channel);
if (m_direction == Direction.Both || m_direction == Direction.Forward)
{
s_relayChannels.Allocate(m_channel * 2, "Relay channel " + m_channel + " is already allocated");
HAL.Base.HAL.Report(ResourceType.kResourceType_Relay, (byte)m_channel);
}
if (m_direction == Direction.Both || m_direction == Direction.Reverse)
{
s_relayChannels.Allocate(m_channel * 2 + 1, "Relay channel " + m_channel + " is already allocated");
HAL.Base.HAL.Report(ResourceType.kResourceType_Relay, (byte)(m_channel + 128));
}
int status = 0;
m_port = HALDigital.InitializeDigitalPort(HAL.Base.HAL.GetPort((byte)m_channel), ref status);
CheckStatus(status);
m_safetyHelper = new MotorSafetyHelper(this);
m_safetyHelper.SafetyEnabled = false;
LiveWindow.LiveWindow.AddActuator("Relay", m_channel, this);
}
private void SetupMotorSafety()
{
SafetyHelper = new MotorSafetyHelper(this)
{
Expiration = DefaultExpirationTime,
SafetyEnabled = true
};
}
/// <summary>
/// Creates a new <see cref="CANJaguar"/> with a specific device number.
/// </summary>
/// <param name="deviceNumber">The CAN Id of the Jaguar.</param>
public CANJaguar(int deviceNumber)
{
s_allocated.Allocate(deviceNumber - 1, "CANJaguar device " + deviceNumber + " is already allocated");
m_deviceNumber = (byte)deviceNumber;
m_controlMode = ControlMode.PercentVbus;
m_safetyHelper = new MotorSafetyHelper(this);
bool receivedFirmwareVersion = false;
byte[] data = new byte[8];
RequestMessage(CAN_IS_FRAME_REMOTE | CAN_MSGID_API_FIRMVER);
RequestMessage(LM_API_HWVER);
for (int i = 0; i < ReceiveStatusAttempts; i++)
{
Timer.Delay(0.001);
SetupPeriodicStatus();
UpdatePeriodicStatus();
if (!receivedFirmwareVersion)
{
try
{
GetMessage(CAN_MSGID_API_FIRMVER, CAN_MSGID_FULL_M, data);
m_firmwareVersion = UnpackInt32(data);
receivedFirmwareVersion = true;
}
catch (CANMessageNotFoundException)
{
}
}
if (m_receivedStatusMessage0 &&
m_receivedStatusMessage1 &&
m_receivedStatusMessage2 &&
receivedFirmwareVersion)
{
break;
}
}
if (!m_receivedStatusMessage0 ||
!m_receivedStatusMessage1 ||
!m_receivedStatusMessage2 ||
!receivedFirmwareVersion)
{
Dispose();
throw new CANMessageNotFoundException();
}
try
{
GetMessage(LM_API_HWVER, CAN_MSGID_FULL_M, data);
m_hardwareVersion = data[0];
}
catch (CANMessageNotFoundException)
{
m_hardwareVersion = 0;
}
if (m_firmwareVersion >= 3330 || m_firmwareVersion < 108)
{
if (m_firmwareVersion < 3330)
{
DriverStation.ReportError("Jag " + m_deviceNumber + " firmware " + m_firmwareVersion + " is too old (must be at least version 108 of the FIRST approved firmware)", false);
}
else
{
DriverStation.ReportError("Jag" + m_deviceNumber + " firmware " + m_firmwareVersion + " is not FIRST approved (must be at least version 108 of the FIRST approved firmware)", false);
}
return;
}
}
/// <inheritdoc/>
public void Dispose()
{
s_allocated.Deallocate(m_deviceNumber - 1);
m_safetyHelper = null;
int status = 0;
int messageId;
switch (m_controlMode)
{
case ControlMode.PercentVbus:
messageId = (int)m_controlMode | LM_API_VOLT_T_SET;
break;
case ControlMode.Speed:
messageId = (int)m_controlMode | LM_API_SPD_T_SET;
break;
case ControlMode.Position:
messageId = (int)m_controlMode | LM_API_POS_T_SET;
break;
case ControlMode.Current:
messageId = (int)m_controlMode | LM_API_ICTRL_T_SET;
break;
case ControlMode.Voltage:
messageId = (int)m_controlMode | LM_API_VCOMP_T_SET;
break;
default:
return;
}
FRC_NetworkCommunication_CANSessionMux_sendMessage((uint)messageId, null, 0,
CAN_SEND_PERIOD_STOP_REPEATING, ref status);
}
/// <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);
}
