void UpdateUniversalRobotStatus() { Stream stream = m_URTcpClient.GetStream(); //UR robot v5.4 returns a 1116 byte packet //Grab the last 10 packets and parse the newest one byte[] allBuffer = new byte[(10 * UNIVERSAL_ROBOT_TCP_STATUS_BUFFER_SIZE)]; int k = stream.Read(allBuffer, 0, (10 * UNIVERSAL_ROBOT_TCP_STATUS_BUFFER_SIZE)); int startIndex = k - UNIVERSAL_ROBOT_TCP_STATUS_BUFFER_SIZE; byte[] bb = new byte[UNIVERSAL_ROBOT_TCP_STATUS_BUFFER_SIZE]; Array.Copy(allBuffer, startIndex, bb, 0, UNIVERSAL_ROBOT_TCP_STATUS_BUFFER_SIZE); FlipEndian(typeof(UniversalRobotRealTimeTCPStatus), bb); GCHandle handle = GCHandle.Alloc(bb, GCHandleType.Pinned); UniversalRobotRealTimeTCPStatus robotStatus = (UniversalRobotRealTimeTCPStatus)Marshal.PtrToStructure(handle.AddrOfPinnedObject(), typeof(UniversalRobotRealTimeTCPStatus)); if (IsGoodStatusPacket(robotStatus)) { if (robotStatus.QD_Actual_1 != URRobotStatus.QD_Actual_1 && robotStatus.QD_Actual_1 == (double)0) { OnPropertyChanged("ProgramState"); } URRobotStatus = robotStatus; } handle.Free(); }
bool IsGoodStatusPacket(UniversalRobotRealTimeTCPStatus status) { double outOfRange = 5; double outOfRangeAngle = 100; if (Math.Abs(status.ToolVectorActual_1) > outOfRange || Math.Abs(status.ToolVectorActual_2) > outOfRange || Math.Abs(status.ToolVectorActual_3) > outOfRange || Math.Abs(status.ToolVectorActual_4) > outOfRangeAngle || Math.Abs(status.ToolVectorActual_5) > outOfRangeAngle || Math.Abs(status.ToolVectorActual_6) > outOfRangeAngle ) { return(false); } return(true); }