private static void print_status(status_t status) { Console.WriteLine("rpm: {0} pos: {1} flags: {2}", status.CurSpeed, status.CurPosition, status.Flags); }
public static extern Result get_status(int id, out status_t status);
/// <summary> /// Checks for a wide variety of possible errors, handles, logs, and prints them on the user interface notification area /// <br/>See Entities.Enums.ControllerError for all possible error types /// <br/>See Entities.Enums.NotificationType for all possible notification types /// </summary> /// <param name="status">object which holds relevant data regarding to the actuators status, of type status_t</param> public void CheckForActuatorErrors(status_t status) { string errorMessage; switch (status.EncSts) { case 0x2: LUT.Messages.TryGetValue(Enums.ControllerError.EncoderStateMalfunction, out errorMessage); logController.LogControllerError(Enums.ControllerError.EncoderStateMalfunction, status.EncSts, errorMessage); Notification.NotifList.Add(new Notification(Enums.NotificationType.Warning, errorMessage)); HandlePhysicalErrors(Enums.PhysicalErrorType.Warning, errorMessage); break; default: break; } switch (status.Flags ^ 0x40) { case 0x1: LUT.Messages.TryGetValue(Enums.ControllerError.CommandError, out errorMessage); logController.LogControllerError(Enums.ControllerError.CommandError, status.Flags ^ 0x40, errorMessage); Notification.NotifList.Add(new Notification(Enums.NotificationType.Warning, errorMessage)); HandlePhysicalErrors(Enums.PhysicalErrorType.Warning, errorMessage); break; case 0x2: LUT.Messages.TryGetValue(Enums.ControllerError.DataIntegrityError, out errorMessage); logController.LogControllerError(Enums.ControllerError.DataIntegrityError, status.Flags ^ 0x40, errorMessage); Notification.NotifList.Add(new Notification(Enums.NotificationType.Warning, errorMessage)); HandlePhysicalErrors(Enums.PhysicalErrorType.Warning, errorMessage); break; case 0x4: LUT.Messages.TryGetValue(Enums.ControllerError.ValueError, out errorMessage); logController.LogControllerError(Enums.ControllerError.ValueError, status.Flags ^ 0x40, errorMessage); Notification.NotifList.Add(new Notification(Enums.NotificationType.Warning, errorMessage)); HandlePhysicalErrors(Enums.PhysicalErrorType.Warning, errorMessage); break; case 0x100: LUT.Messages.TryGetValue(Enums.ControllerError.PowerDriverOverheat, out errorMessage); logController.LogControllerError(Enums.ControllerError.PowerDriverOverheat, status.Flags ^ 0x40, errorMessage); Notification.NotifList.Add(new Notification(Enums.NotificationType.CriticalError, errorMessage)); HandlePhysicalErrors(Enums.PhysicalErrorType.Critical, errorMessage); break; case 0x200: LUT.Messages.TryGetValue(Enums.ControllerError.PowerControllerOverheat, out errorMessage); logController.LogControllerError(Enums.ControllerError.PowerControllerOverheat, status.Flags ^ 0x40, errorMessage); Notification.NotifList.Add(new Notification(Enums.NotificationType.CriticalError, errorMessage)); HandlePhysicalErrors(Enums.PhysicalErrorType.Critical, errorMessage); break; case 0x400: LUT.Messages.TryGetValue(Enums.ControllerError.PowerVolatageOverload, out errorMessage); logController.LogControllerError(Enums.ControllerError.PowerVolatageOverload, status.Flags ^ 0x40, errorMessage); Notification.NotifList.Add(new Notification(Enums.NotificationType.CriticalError, errorMessage)); HandlePhysicalErrors(Enums.PhysicalErrorType.Critical, errorMessage); break; case 0x800: LUT.Messages.TryGetValue(Enums.ControllerError.PowerCurrentOverload, out errorMessage); logController.LogControllerError(Enums.ControllerError.PowerCurrentOverload, status.Flags ^ 0x40, errorMessage); Notification.NotifList.Add(new Notification(Enums.NotificationType.CriticalError, errorMessage)); HandlePhysicalErrors(Enums.PhysicalErrorType.Critical, errorMessage); break; case 0x1000: LUT.Messages.TryGetValue(Enums.ControllerError.UsbVoltageOverload, out errorMessage); logController.LogControllerError(Enums.ControllerError.UsbVoltageOverload, status.Flags ^ 0x40, errorMessage); Notification.NotifList.Add(new Notification(Enums.NotificationType.CriticalError, errorMessage)); HandlePhysicalErrors(Enums.PhysicalErrorType.Critical, errorMessage); break; case 0x4000: LUT.Messages.TryGetValue(Enums.ControllerError.UsbCurrentOverload, out errorMessage); logController.LogControllerError(Enums.ControllerError.UsbCurrentOverload, status.Flags ^ 0x40, errorMessage); Notification.NotifList.Add(new Notification(Enums.NotificationType.CriticalError, errorMessage)); HandlePhysicalErrors(Enums.PhysicalErrorType.Critical, errorMessage); break; case 0x400000: LUT.Messages.TryGetValue(Enums.ControllerError.CurrentLimitExceeded, out errorMessage); logController.LogControllerError(Enums.ControllerError.CurrentLimitExceeded, status.Flags ^ 0x40, errorMessage); Notification.NotifList.Add(new Notification(Enums.NotificationType.CriticalError, errorMessage)); HandlePhysicalErrors(Enums.PhysicalErrorType.Critical, errorMessage); break; default: break; } switch (status.GPIOFlags) { case 0x1: LUT.Messages.TryGetValue(Enums.ControllerError.LeftLimitReached, out errorMessage); logController.LogControllerError(Enums.ControllerError.LeftLimitReached, status.GPIOFlags, errorMessage); Notification.NotifList.Add(new Notification(Enums.NotificationType.Warning, errorMessage)); HandlePhysicalErrors(Enums.PhysicalErrorType.LeftEdge, errorMessage); break; case 0x2: LUT.Messages.TryGetValue(Enums.ControllerError.RightLimitReached, out errorMessage); logController.LogControllerError(Enums.ControllerError.RightLimitReached, status.GPIOFlags, errorMessage); Notification.NotifList.Add(new Notification(Enums.NotificationType.Warning, errorMessage)); HandlePhysicalErrors(Enums.PhysicalErrorType.RightEdge, errorMessage); break; default: break; } if (status.Upwr <= 0) { LUT.Messages.TryGetValue(Enums.ControllerError.NoVoltage, out errorMessage); logController.LogControllerError(Enums.ControllerError.NoVoltage, status.GPIOFlags, errorMessage); Notification.NotifList.Add(new Notification(Enums.NotificationType.CriticalError, errorMessage)); HandlePhysicalErrors(Enums.PhysicalErrorType.Critical, errorMessage); } }
/// <summary> /// Searches for string identifiers and decodes if packet is found. /// Main part of RX functionality. /// </summary> /// <returns>The status after trying to decode any characters present in the RX buffer.</returns> public status_t Run() { status_t status = new status_t(); // Read data from serial buffer into private buffer _rxBuffer += _port.serialPort.ReadExisting(); // Check for empty buffer (no data yet received) if (_rxBuffer == "") { return(statusArray.ok); } // Remove non-alpha numeric characters from start while (Char.IsLetterOrDigit(_rxBuffer, 0) == false) { // Remove first element _rxBuffer = _rxBuffer.Remove(0, 1); // Re-check if empty if (_rxBuffer == "") { return(statusArray.ok); } } // Check for too-large buffer if (_rxBuffer.Length >= _buffFlushLimit) { // Reset buffer if too large (including start/end counters) this.ResetBuffer(ref _startPos, ref _endPos); _decodeState = DecodeStates.Idle; // Return error return(statusArray.rxBufferOverflow); } // Set finished flag to false on entry to state machine bool finishedFlag = false; while (!finishedFlag) { // State machine for search and decode algorithm switch (_decodeState) { case DecodeStates.Idle: if (_isStartChar) { // Try and find the first instance of the start packet identifier _startPos = _rxBuffer.IndexOf(_startChar); //Debug.WriteLine("IDL: Start Pos = " + startPos.ToString()); if (_startPos >= 0) { _decodeState = DecodeStates.StartCharFound; } else { // No start-character found, so clear buffer (only if not already 0 to avoid errors) if (_rxBuffer.Length > 0) { this.ResetBuffer(ref _startPos, ref _endPos); } // Remain in current searchAndDecodeState // Return o.k. status = statusArray.ok; finishedFlag = true; } } else { // No start char, so jump straight into the next state _startPos = 0; _decodeState = DecodeStates.StartCharFound; } break; case DecodeStates.StartCharFound: // Search for end-character (starting for 1 after the start-character) _endPos = _rxBuffer.IndexOf(_endIdenChar, _startPos + 1); //Debug.WriteLine("SCF: Start Pos = " + startPos.ToString()); // Check to see if end character was found if (_endPos > _startPos) { _decodeState = DecodeStates.EndCharFound; break; } else { // Stay in same searchAndDecodeState (do nothing) finishedFlag = true; status = statusArray.ok; break; } case DecodeStates.EndCharFound: // Make sure there is not another start character before the end character // (which would indicate a truncated packet for some reason) // Only valid if there is a start character if (_isStartChar) { int nextStartChar = _rxBuffer.IndexOf(_startChar, _startPos + 1); if (nextStartChar > 0 && nextStartChar < _endPos) { // Unexpected truncation found _decodeState = DecodeStates.Idle; // Remove everything in buffer up to the next start char _rxBuffer = _rxBuffer.Remove(0, nextStartChar); status = statusArray.packetUnexpectedlyTruncated; finishedFlag = true; break; } } if (_isHeader) { // If so, check for dataID straight after the start character (exact match returns 0) if (String.Compare(_rxBuffer, _startPos + 1, _dataIdString, 0, _dataIdString.Length) == 0) { // If found, switch searchAndDecodeState _decodeState = DecodeStates.IdFound; break; } else { // Data ID not recognized, even though start and end characters found. // Reset buffer, go back to idle searchAndDecodeState and return error this.ResetBuffer(ref _startPos, ref _endPos); // Reset back to idle state _decodeState = DecodeStates.Idle; // Set error status status = statusArray.dataIdDidNotMatch; // Exit loop finishedFlag = true; break; } } { // No header so directly go to the searchAndDecodeState _decodeState = DecodeStates.IdFound; break; } case DecodeStates.IdFound: // Start, end characters and data id match, time to extract data if (this.DecodeCmdString(_rxBuffer.Substring(_startPos, _endPos - _startPos + 1)) == true) { // Goto finished state _decodeState = DecodeStates.Decoded; break; } else { // IF FAIL // Remove the packet and everything before it from the buffer _rxBuffer = _rxBuffer.Remove(0, _endPos + 1); // Go back to idle searchAndDecodeState _decodeState = DecodeStates.Idle; status = statusArray.unrecognisedCmd; finishedFlag = true; break; } // Packet has been decoded, now to check all parameters are // present case DecodeStates.Decoded: // Check that all the required parameters were present, // and if so, go to the next state. if (this.CheckParameters()) { _decodeState = DecodeStates.RunCallBackFunc; break; } else { // IF FAIL // Remove the packet and everything before it from the buffer _rxBuffer = _rxBuffer.Remove(0, _endPos + 1); // Go back to idle searchAndDecodeState _decodeState = DecodeStates.Idle; status = statusArray.incorrectNumParameters; finishedFlag = true; break; } break; // Run callback function case DecodeStates.RunCallBackFunc: this.RunCallBackFunction(); _decodeState = DecodeStates.Finished; break; // Packet decoding finished successfully! case DecodeStates.Finished: // IF PASS // Increment packet decoded variable _packetCount++; // Remove the packet and everything before it from the buffer _rxBuffer = _rxBuffer.Remove(0, _endPos + 1); // Go back to idle searchAndDecodeState _decodeState = DecodeStates.Idle; // Else return o.k. status = statusArray.packetDecodingPassed; finishedFlag = true; break; } } return(status); }