Common.cs
14.4 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
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
using log4net;
using log4net.Util;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;
namespace Mushiny
{
public class Common
{
public static string Mushiny_ = "Mushiny_";
public static readonly ILog LOGGER = LogManager.GetLogger("Mushiny");
//public static unsafe ushort crc16(byte[] bytes, int len)
//{
// byte* buf = (byte*)Marshal.AllocHGlobal(len);
// Marshal.Copy(bytes, 0, (IntPtr)buf, len);
// byte uchCRCHi = 0xFF; // 采用的模式是初始FFFF
// byte uchCRCLo = 0xFF; //
// uint uIndex; // CRC循环中的索引
// while ((len--) >= 0)
// {
// uIndex = (uint)(uchCRCLo ^ (*buf++)); //计算CRC
// uchCRCLo = (byte)(uchCRCHi ^ auchCRCHi[uIndex]);
// uchCRCHi = auchCRCLo[uIndex];
// }
// Marshal.FreeHGlobal((IntPtr)buf);
// return (ushort)(uchCRCHi << 8 | uchCRCLo);
//}
/// <summary>
///CRC16-MaxIM多项式是： 8005 x16+x15+x2+1
/// </summary>
/// <param name="buffer"></param>
/// <param name="length"></param>
/// <param name="startPos"></param>
/// <returns></returns>
public static ushort CRC_16(byte[] buffer, int length, int startPos)
{
byte crcHi = 0xff; // 高位初始化
byte crcLo = 0xff; // 低位初始化
for (int i = startPos; i < (length + startPos); i++)
{
int crcIndex = crcHi ^ buffer[i]; //查找crc表值
crcHi = (byte)(crcLo ^ auchCRCHi[crcIndex]);
crcLo = auchCRCLo[crcIndex];
}
return (ushort)(crcHi << 8 | crcLo);
}
// CRC16 查表校验法
// CRC 高位字节值表
static byte[] auchCRCHi = {
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40
};
// CRC低位字节值表
static byte[] auchCRCLo = {
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06,
0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD,
0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A,
0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4,
0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3,
0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4,
0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29,
0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED,
0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60,
0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,
0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68,
0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E,
0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B,
0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B,
0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,
0x43, 0x83, 0x41, 0x81, 0x80, 0x40
};
public static byte[] hexStringToBytes(string hexString)
{
if (hexString == null || hexString.Equals(""))
{
return null;
}
hexString = hexString.Replace(" ", "");
if ((hexString.Length % 2) != 0)
{
hexString += " ";
}
byte[] rtnBytes = new byte[hexString.Length / 2];
for (int i = 0; i < rtnBytes.Length; i++)
{
rtnBytes[i] = Convert.ToByte(hexString.Substring(i * 2, 2), 16);
}
return rtnBytes;
}
public static string BytesTostr(byte[] bytes)
{
string rtnStr = "";
if (bytes != null)
{
for (int i = 0; i < bytes.Length; i++)
{
rtnStr += " ";
rtnStr += bytes[i].ToString("X2");
}
}
return rtnStr.Trim();
}
public static bool GetFaultId(byte[] bytes, out FaultId faultId)
{
faultId = FaultId.无;
if (bytes == null || bytes.Length == 0) return true;
try
{
if (bytes.Length == 2)
{
var high = bytes[0];
var low = bytes[1];
var val = (high << 8) | (low);
faultId = (FaultId)val;
}
else
{
return false;
}
}
catch
{
return false;
}
return true;
}
public static byte[] ConvertToBytes(string val, int len)
{
var strBytes = System.Text.Encoding.ASCII.GetBytes(val);
if (strBytes == null)
{
return new byte[len];
}
else
{
if (strBytes.Length < len)
{
var lst = new List<byte>();
for (int i = 1; i <= len - strBytes.Length; i++)
{
lst.Add(0);
}
lst.AddRange(strBytes);
return lst.ToArray();
}
else if (strBytes.Length > len)
{
var lst = new List<byte>();
for (int i = 0; i <= len - 1; i++)
{
lst.Add(strBytes[i]);
}
return lst.ToArray();
}
else
{
return strBytes;
}
}
}
public static string ConvertToStr(byte[] val)
{
return System.Text.Encoding.ASCII.GetString(getShowedBytes(val));
}
static byte[] getShowedBytes(byte[] bytes)
{
//ASCII字符集由95个可打印字符（0x20-0x7E）和33个控制字符（0x00-0x1F，0x7F）组成
var lst = new List<byte>();
if (bytes != null)
{
foreach (var item in bytes)
{
if (item >= 0x20 && item <= 0x7e)
{
lst.Add(item);
}
}
}
return lst.ToArray();
}
/// <summary>
/// 转换为2字节
/// <param name="val"></param>
/// <returns></returns>
public static byte[] ConvertToBytes(ushort val)
{
return BitConverter.GetBytes(val);
}
public static ushort ConvertToUshort(byte[] bytes)
{
return BitConverter.ToUInt16(bytes, 0);
}
public static short ConvertToShort(byte[] bytes)
{
return BitConverter.ToInt16(bytes, 0);
}
/// <summary>
/// 转换为4字节
/// </summary>
/// <param name="val"></param>
/// <returns></returns>
public static byte[] ConvertToBytes(uint val)
{
return BitConverter.GetBytes(val);
}
public static uint ConvertToUInt(byte[] bytes)
{
return BitConverter.ToUInt32(bytes, 0);
}
public static int ConvertToInt(byte[] bytes)
{
return BitConverter.ToInt32(bytes, 0);
}
public static byte[] ConvertToBytes(float val)
{
return BitConverter.GetBytes(val);
}
public static byte[] ConvertToBytes(short val)
{
return BitConverter.GetBytes(val);
}
public static float ConvertToFloat(byte[] bytes)
{
return BitConverter.ToSingle(bytes, 0);
}
//public static unsafe byte* ChangeType(byte[] bytes)
//{
// byte* bp = (byte*)Marshal.AllocHGlobal(bytes.Count());
// Marshal.Copy(bytes, 0, (IntPtr)bp, 5);
// Marshal.FreeHGlobal((IntPtr)bp);
// return bp;
//}
public static byte[] GetSubBytes(byte[] bytes, int startIdx, int len)
{
byte[] dstArray;
dstArray = new byte[len];
Array.Copy(bytes, startIdx, dstArray, 0, len);
return dstArray;
}
}
/// <summary>
/// 动作完成码
/// </summary>
public enum ActionFinishCode : byte
{
初始化自检或被动查询完成 = 0x01,
直行路径完成,
转弯完成,
旋转伸缩归零完成,
从料仓取货完成,
往料仓放货完成,
从货架取货完成,
往货架放货完成,
扫描货架和料箱完成,
取放货设备对接完成 = 0x0b,
充电对接完成,
充满电,
停到最近二维码完成,
整个路径执行完成,
}
/// <summary>
/// 当前状态字段
/// </summary>
public enum RobotState : byte
{
空闲状态,
直行状态,
预留,
转弯状态,
预留1,
预留2,
预留3,
充电状态,
低功耗状态,
定时暂停状态,
旋转标定状态,
料仓或货架取放货状态,
取放货设备对接状态,
丢失地址码状态 = 0xf0,
IMU异常状态 = 0xf1,
故障状态 = 0xff,
}
/// <summary>
/// 传感器状态标志字段
/// </summary>
public enum SensorState : uint
{
伸缩前侧限位信号 = 0x00000001,
伸缩后侧限位信号_零位 = 0x00000002,
货叉料箱检测信号 = 0x00000004,
挡板状态信号 = 0x00000008,
旋转左侧限位信号 = 0x00000010,
旋转中间信号_零位 = 0x00000020,
旋转右侧限位信号 = 0x00000040,
货架摄像头失联信号 = 0x00000080,
料仓1检测信号 = 0x00000100,
料仓2检测信号 = 0x00000200,
料仓3检测信号 = 0x00000400,
料仓4检测信号 = 0x00000800,
料仓5检测信号 = 0x00001000,
料仓6检测信号 = 0x00002000,
料仓7检测信号 = 0x00004000,
料仓8检测信号 = 0x00008000,
料仓9检测信号 = 0x00010000,
料仓10检测信号 = 0x00020000,
升降上限位信号 = 0x00040000,
升降下限位信号 = 0x00080000,
前侧雷达输入信号1 = 0x00100000,
前侧雷达输入信号2 = 0x00200000,
前侧雷达输入信号3 = 0x00400000,
后侧雷达输入信号1 = 0x00800000,
后侧雷达输入信号2 = 0x01000000,
后侧雷达输入信号3 = 0x02000000,
前触边信号 = 0x04000000,
后触边信号 = 0x08000000,
升降夹手触边信号 = 0x10000000,
宽度调节外侧限位信号 = 0x20000000,
高温检测信号_超过50度 = 0x40000000,
烟雾检测信号 = 0x80000000
}
/// <summary>
/// 故障标志位字段
/// </summary>
public enum FaultFlag : uint
{
左轮电机故障 = 0x00000001,
右轮电机故障 = 0x00000002,
升降电机故障 = 0x00000004,
伸缩电机故障 = 0x00000008,
旋转电机故障 = 0x00000010,
挡杆电机故障 = 0x00000020,
宽度调节电机故障 = 0x00000040,
电池故障 = 0x00000080,
下视摄像头失联标志 = 0x00000100,
货叉摄像头失联标志 = 0x00000200,
上主控失联 = 0x00000400,
外设模块失联 = 0x00000800,
拉线编码器故障 = 0x00001000,
IMU惯导故障 = 0x00002000,
避障雷达故障 = 0x00004000,
网络失联标志 = 0x00008000,
料仓货位传感器异常 = 0x00010000,
升降上限位传感器异常 = 0x00020000,
升降下限位传感器异常 = 0x00040000,
伸缩限位传感器异常 = 0x00080000,
货叉货位传感器异常 = 0x00100000,
旋转中间信号零位异常 = 0x00200000,
旋转左侧限位信号异常 = 0x00400000,
旋转右侧限位信号异常 = 0x00800000,
挡杆动作超时异常 = 0x01000000,
货叉避障传感器触发 = 0x02000000,
货叉安全触边触发 = 0x04000000,
前安全触边触发 = 0x08000000,
后安全触边触发 = 0x10000000,
底盘环境高温报警 = 0x20000000,
烟雾检测信号 = 0x40000000,
系统故障 = 0x80000000
}
}