URRobotManager.cs
4.2 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
using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using URSoldering.Common;
namespace URSoldering.DeviceLibrary
{
public class URRobotManager
{
private static URTcpClient listenClient = null;
public static void StartListen()
{
listenClient = new URTcpClient();
listenClient.DefaultDataLength = 1024;
listenClient.ReviceSleepMS = 5;
listenClient.connect("192.168.10.10", 30003, HandleMessage);
}
public static void StopListen()
{
if (listenClient != null)
{
listenClient.close();
}
}
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)
{
//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);
for (int i = 0; i < maxdouble; i++)
{
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;
}
}
if (doubleList.Count > 100)
{
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;
LogUtil.info("数据长度："+messageSize+"，data长度："+reviceData.Length+"。实际坐标：" + x + spilt + y + spilt + z + spilt + rx + spilt + ry + spilt + rz + spilt);
}
else
{
string reviceMsg = ByteToStr(reviceData);
LogUtil.info("Read data：【" + reviceMsg + "】 ");
LogUtil.info("无法获取坐标，数据长度不正确");
}
}catch(Exception ex)
{
LogUtil.error("出错：" + ex.ToString());
}
}
}
}