URRobotClient.cs
5.5 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using URSoldering.Common;
namespace URSoldering.DeviceLibrary
{
public class URRobotClient
{
private static URTcpClient listenClient = null;
private static int Port = 30003;
public static string LastMoveCMD = "";
public static void StartListen(string ip)
{
listenClient = new URTcpClient();
listenClient.DefaultDataLength = 1078;
listenClient.ReviceSleepMS = 5;
listenClient.connect(ip, Port, HandleMessage);
}
public static void StopListen()
{
if (listenClient != null)
{
listenClient.close();
}
}
public static bool IsConnected()
{
if (listenClient != null)
{
return listenClient.IsConnected();
}return false;
}
public static string LogName
{
get { return "【UR端口：" + Port + "】"; }
}
private static double BitToDouble(byte[] data)
{
double f = BitConverter.ToDouble(data, 0);
return f;
}
private static int BitToInt(byte[] data)
{
int f = BitConverter.ToInt32(data, 0);
return f;
}
private static double BitToFloat(string hexString)
{
uint num = uint.Parse(hexString, System.Globalization.NumberStyles.AllowHexSpecifier);
byte[] floatVals = BitConverter.GetBytes(num);
double f = BitConverter.ToDouble(floatVals, 0);
return f;
}
private static string ByteToStr(byte[] reviceData)
{
string strFromat = "{0:X2}";
string reviceMsg = "";
foreach (byte data in reviceData)
{
reviceMsg = reviceMsg + " " + String.Format(strFromat, data);
}return reviceMsg;
}
private static void HandleMessage(byte[] reviceData)
{
if (LastMoveCMD.Equals("").Equals(false))
{
listenClient.sendLine(LastMoveCMD);
LastMoveCMD = "";
}
StopListen();
try
{
//string reviceMsg = ByteToStr(reviceData);
//LogUtil.info("Read data：【" + reviceMsg + "】 ");
byte[] msgSizeArray = reviceData.Skip(0).Take(4).ToArray();
byte[] timeArray = reviceData.Skip(4).Take(8).ToArray();
byte[] qTargetArray = reviceData.Skip(12).Take(48).ToArray();
byte[] qdTargetArray = reviceData.Skip(60).Take(48).ToArray();
byte[] qddArray = reviceData.Skip(108).Take(48).ToArray();
byte[] iTargetArray = reviceData.Skip(156).Take(48).ToArray();
int messageSize = BitToInt(reviceData.Skip(0).Take(4).ToArray().Reverse<byte>().ToArray());
int maxdouble = (messageSize - 4) / 8;
List<double> doubleList = new List<double>();
doubleList.Add(messageSize);
if (maxdouble > 61)
{
for (int i = 0; i < maxdouble; i++)
{
if (i >= 55 && i <= 61)
{
if (reviceData.Length >= (12 + i * 8))
{
byte[] data = reviceData.Skip(4 + i * 8).Take(8).ToArray().Reverse<byte>().ToArray();
double value = BitToDouble(data);
//LogUtil.info("第【"+(i+1)+"】个double["+ByteToStr(data) +"]值："+value);
doubleList.Add(value);
}
else
{
break;
}
}
else
{
doubleList.Add(0);
}
}
if (doubleList.Count > 61)
{
string spilt = "，";
double x = doubleList[56] * 1000;
double y = doubleList[57] * 1000;
double z = doubleList[58] * 1000;
double rx = doubleList[59] * 1;
double ry = doubleList[60] * 1;
double rz = doubleList[61] * 1;
URPointValue newp = new URPointValue(x, y, z, rx, ry, rz);
URRobotControl.LastPoint = newp;
string reviceMsg = ByteToStr(reviceData);
//LogUtil.info("Read data：【" + reviceMsg + "】 ");
LogUtil.URLInfo( LogName+"length[" + messageSize + "]，data长[" + reviceData.Length + "]坐标"+newp.ToShowStr());
}
}
else
{
string reviceMsg = ByteToStr(reviceData);
LogUtil.URLError(LogName + "Read data：【" + reviceMsg + "】 ");
LogUtil.URLError(LogName + "无法获取坐标，数据长度不正确");
}
}catch(Exception ex)
{
LogUtil.URLError(LogName+ "HandleMessage出错：" + ex.ToString());
StopListen();
}
}
}
}