DisplacementSensor.c
1.75 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
#include "DisplacementSensor.h"
//02 06 00 4C 00 00 48 2E
u8 setLiftZero[8] = {0x02,0x06,0x00,0x4C,0x00,0x00,0x48,0x2E};
u8 getLiftData[8] = {0x02,0x03,0x00,0x00,0x00,0x02,0xC4,0x38};
void getLiftHeight()
{
static int i = 0;
if(i++ >= 6)
{
i = 0;
WriteUart(COM4,getLiftData,8);
}
}
void SetLiftHeightZero()
{
static int i = 0;
if(i++ > 10)
{
i = 0;
WriteUart(COM4,setLiftZero,8);
}
}
uint16_t AddCheckCRC2(unsigned char* cBuffer, short iBufLen)
{
uint16_t i = 0, j = 0;
uint16_t wCrc = 0xffff;
uint16_t wPolynom = 0xA001;
for (i = 0; i < iBufLen; i++)
{
wCrc ^= cBuffer[i];
for (j = 0; j < 8; j++)
{
if (wCrc & 0x0001)
{
wCrc = (wCrc >> 1) ^ wPolynom;
}
else
{
wCrc = wCrc >> 1;
}
}
}
return wCrc;
}
void liftDataProcess1(unsigned char *Res)
{
static unsigned char Buffer2[20];
static unsigned short iii = 0;
static unsigned char step = 0;
unsigned short crcValue = 0;
static int LiftHeight = 0;
if (iii > 9)
{
iii = 0;
step = 0;
}
if (step == 0 && *Res == 0x02) // $
{
iii = 0;
step = 1;
}
else if(step == 1 && Buffer2[1] == 0x03)
{
step = 2;
}
else if(step == 2 && Buffer2[2] == 0x04)
{
if(iii == 9)//½ÓÊÕÍê
{
step = 0;
crcValue = Buffer2[7]|Buffer2[8]<<8;
if(AddCheckCRC2(Buffer2,7) == crcValue)
{
DriverLifter1.Public.encoderPose = (Buffer2[3]<<24|Buffer2[4]<<16|Buffer2[5]<<8|Buffer2[6])/10 + agv.Parameter.LiftCompensation;//µ¥Î»MM
LiftHeight = Buffer2[3]<<24|Buffer2[4]<<16|Buffer2[5]<<8|Buffer2[6];
}
}
}
Buffer2[iii++] = *Res;
}
void liftEncoderDataProcess()//2444 1240
{
unsigned char buff[20];
unsigned int len;
unsigned int i;
len = ReadUart(COM4, buff, 20);
if(len > 0)
{
for (i = 0; i < len; i++)
{
liftDataProcess1(buff+i);
}
}
}