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