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 }; }
/// <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; } }
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); }
/// <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); }