Commun_TCP.c
7.34 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
#include "includes.h"
#include "hardware.h"
#include "all_value.h"
u8 remote1_addr[4];
u16 remote1_Port;
u8 TCP_Mode;
u8 TCP_AcNo;
s8 TCP_DX_LTAdjust,TCP_DY_LTAdjust;
u16 Tcp_Weight;
u8 Tim5_TCP1Nums;//通讯异常时间计数
Str_TaskPoint TaskP[4];//任务点
void TCP_RX_Data_Analysis(void) //TCP通讯接收数据解析
{
short VaS16;
u8 i;
Tim5_TCP1Nums=0;
if(tcp_client_recvbuf[0]==79)
{ if(Workmode==AutoMode)
{
if( tcp_client_recvbuf[tcp_client_recvbuf[0]-1]==XOR_Crc(tcp_client_recvbuf,(tcp_client_recvbuf[0]-1),0) )
{
if( UnSame_Campare(AGV.TaskNo,tcp_client_recvbuf+1,4))//若任务号更新则表示有新任务
{
memset(&AGV,0,sizeof(AGV));
AGV.Task_Step=1;
memcpy(AGV.TaskNo,tcp_client_recvbuf+1,4);//任务号
}
else if(AGV.Task_Step==100)//任务号相同，继续执行
{ AGV.RouteP[0].Code=0;
}
if(tcp_client_recvbuf[5])
{
memset(&AGV.TaskNo,0,sizeof(AGV.TaskNo));
if(Motor[0].MToPc.speed<20 && Motor[1].MToPc.speed<20&& Motor[3].MToPc.speed<20)
{ MessSpecialClear();}
}//清除任务
for(i=0;i<4;i++)
{
memcpy(&TaskP[i].Code,tcp_client_recvbuf+6+i*18,2); //虚拟码值
memcpy(&VaS16,tcp_client_recvbuf+8+i*18,2);//位置X
TaskP[i].SP.X_Slam=(float)VaS16*10.0f;
memcpy(&VaS16,tcp_client_recvbuf+10+i*18,2);//位置Y
TaskP[i].SP.Y_Slam=(float)VaS16*10.0f;
memcpy(&VaS16,tcp_client_recvbuf+12+i*18,2);//车头指向角度
TaskP[i].SP.Ang=VaS16;
memcpy(&TaskP[i].P_Type1,tcp_client_recvbuf+14+i*18,1);//点属性
memcpy(&TaskP[i].P_AD_Type2,tcp_client_recvbuf+15+i*18,1);//点调节属性
memcpy(&TaskP[i].P_AC_Type3,tcp_client_recvbuf+16+i*18,1);//点动作属性
memcpy(&TaskP[i].Dis,tcp_client_recvbuf+17+i*18,2); //高度/距离
memcpy(&TaskP[i].P_ConveyDir,tcp_client_recvbuf+21+i*18,1); //输送方向
}
tcp_client_sendbuf[0]=0;
Task_Get();
Set_ErrID(4,0);
}
else Set_ErrID(4,1); //ErrID=4;通讯校验位有误
}
/*校验码2对应的内容*/
if( tcp_client_recvbuf[TCP_CLIENT_RX_BUFSIZE-1]==XOR_Crc(tcp_client_recvbuf+tcp_client_recvbuf[0],(TCP_CLIENT_RX_BUFSIZE-tcp_client_recvbuf[0]-1),0) )
{
AGV.OtheSta2=tcp_client_recvbuf[79];
TCP_Mode=tcp_client_recvbuf[80];
TCP_AcNo=tcp_client_recvbuf[81];
TCP_DX_LTAdjust=tcp_client_recvbuf[82];
TCP_DY_LTAdjust=tcp_client_recvbuf[83];
memcpy(&Tcp_Weight,tcp_client_recvbuf+85,2);
}
else
{ TCP_Mode=0;
TCP_AcNo=0;
}
}
}
void Task_Get(void) //获取任务，将任务适当时候放入路径缓存的合适位置中
{
// RouteP
u8 i;
u8 RPE_Index;
u8 TPE_Index;
u8 Array_RP[LengTP];
u8 Array_TP[LengTP];
if( (AGV.RouteP[0].Code==0)&& TaskP[0].Code )//路径缓存(执行缓存)为空时，一次获取任务缓存
{
memcpy(&AGV.RouteP[0].Code,&TaskP[0].Code,LengTP*4);
if(AGV.Task_Step==100)AGV.Task_Step=1;
}
else
{
for(i=0;i<3;i++)//查找路径缓存点中最后的有效点索引
{
if(AGV.RouteP[i].Code && (AGV.RouteP[i+1].Code==0))
{
RPE_Index=i;break;
}
if(i==2)RPE_Index=3;//无空点缓存
}
if(RPE_Index<3)//路径点缓存中有空
{ for(i=0;i<4;i++)
{
if(TaskP[i].Code==AGV.RouteP[RPE_Index].Code)
{ memcpy(Array_RP,&AGV.RouteP[RPE_Index].Code,LengTP);
memcpy(Array_TP,&TaskP[i].Code,LengTP);
if( UnSame_Campare(Array_RP,Array_TP,LengTP )==0 )//比较后一样
{TPE_Index=i;break; }
}
if(i==3)TPE_Index=4;//任务缓存中无路径缓存最后有效点，异常
}
if(TPE_Index<3)//最后有效点对应任务缓存的点，不是任务缓存最后一点
{ RPE_Index=RPE_Index+1;
TPE_Index=TPE_Index+1;
while(RPE_Index<4 && TPE_Index<4 )//跳出条件：点索引超出缓存或者下一个任务点是空
{
if(TaskP[TPE_Index].Code)//下一点非空
{ memcpy(&AGV.RouteP[RPE_Index].Code,&TaskP[TPE_Index].Code,LengTP);
RPE_Index=RPE_Index+1;
TPE_Index=TPE_Index+1;
}
else break;
}
}
}
}
}
#define Delta_Dir 25.0f //方向角偏差允许值
void Sx_MessageDeal(void) //发送信息获取确认
{
/*AGV的状态确认*/
u8 i;
if(VaIndexB_R(&AGV.OtheSta,4))AGV.Status=13;
else if(Workmode==AutoMode )
{ if( Out_Charge && ((AGV.Charge_Step==0)|(AGV.Charge_Step==100)) )AGV.Status=14;
else if( (Status_SlamLaser==2)&&Motor[2].MToPc.Home_Found ) AGV.Status=11;
else AGV.Status=10;}
else if(Workmode==ManuMode)
{ if(Status_SlamLaser==2) AGV.Status=21;
else AGV.Status=20;}
/*AGV的角度大方向判断*/
if(Ang_SlamAgv<(0+Delta_Dir) | Ang_SlamAgv>(360-Delta_Dir))AGV.Dir=1;
else if(Ang_SlamAgv<(90+Delta_Dir) && Ang_SlamAgv>(90-Delta_Dir))AGV.Dir=2;
else if(Ang_SlamAgv<(180+Delta_Dir) && Ang_SlamAgv>(180-Delta_Dir))AGV.Dir=3;
else if(Ang_SlamAgv<(270+Delta_Dir) && Ang_SlamAgv>(270-Delta_Dir))AGV.Dir=4;
else AGV.Dir=0;
/*虚拟点刷新判断*/
for(i=0;i<4;i++)
{ if(TaskP[i].Code && (Status_SlamLaser==2) )
{ if( Dis_CalculateLine(AgvSP,TaskP[i].SP)<250 )
{ AGV.VirCode=TaskP[i].Code;
break;
}
}
}
}
void TCP_SX_Data(void) //将要发送的数据存放至发送缓存中，发送数据刷新
{
// short Va16;
u8 Va8;
u8 Va8_1;
short VaS16;
Sx_MessageDeal();
Tim5_TCP1Nums=0;
memset(tcp_client_sendbuf,0,sizeof(tcp_client_sendbuf));
tcp_client_sendbuf[0]=19;//数据长度
memcpy(tcp_client_sendbuf+1,F_Agvname,8);//本机名称
memcpy(tcp_client_sendbuf+11,&AGV.VirCode,4);//码值
tcp_client_sendbuf[15]=(u8)((float)BatteryVa.Energy*1.06f);//电池电量
tcp_client_sendbuf[16]=AGV.Status;//AGV状态
tcp_client_sendbuf[17]=AGV.Dir;//AGV车头方向
tcp_client_sendbuf[18]=0;//转盘方向
tcp_client_sendbuf[19]=AGV.UDstatus;//升降状态
memcpy(tcp_client_sendbuf+20,&AGV.Err[0],3);//错误1-3
VaS16=0;
memcpy(tcp_client_sendbuf+23,&VaS16,2);//距离
memcpy(tcp_client_sendbuf+25,&AGV.TaskNo[0],4);//任务号
tcp_client_sendbuf[29]=VaIndexB_R(&AGV.OtheSta,3);//完成状态
tcp_client_sendbuf[30]=XOR_Crc(tcp_client_sendbuf,30,0);//校验码1
VaS16=(short)(AgvSP.X_Slam*0.1f);
memcpy(tcp_client_sendbuf+31,&VaS16,2);//当前位置X
VaS16=(short)(AgvSP.Y_Slam*0.1f);
memcpy(tcp_client_sendbuf+33,&VaS16,2);//当前位置Y
if(Ang_SlamLaser>=0 && Status_SlamLaser<=100)Va8=(u8)Status_SlamLaser;
else Va8=0;
tcp_client_sendbuf[35]=Va8;//置信度
VaS16=(short)(Ang_SlamLaser*10);
memcpy(tcp_client_sendbuf+36,&VaS16,2);//当前角度
VaS16=(short)(High_Dis);
memcpy(tcp_client_sendbuf+38,&VaS16,2);//当前高度
// memcpy(tcp_client_sendbuf+40,&IN_Cargo1,1);//货物检测
VaIndexB_W(&Va8_1,0,IN_Cargo1);//上层是否有货物
VaIndexB_W(&Va8_1,1,IN_Cargo2);//下层是否有货物
if(fabs(Motor[5].MToPc.speed)>10)Va8=1;else Va8=0;
VaIndexB_W(&Va8_1,2,Va8);//上层轴运动状态
if(fabs(Motor[0].MToPc.speed)>10)Va8=1;else Va8=0;
VaIndexB_W(&Va8_1,3,Va8);//下层轴运动状态
tcp_client_sendbuf[40]=Va8_1;
memcpy(tcp_client_sendbuf+41,&WeightAll,2);//重量
tcp_client_sendbuf[43]=BatteryVa.Temperature_Max; //电池温度
tcp_client_sendbuf[49]=XOR_Crc(tcp_client_sendbuf+31,18,0);//校验码2
}
u8 TCP_LastAcNo;
void Commun_Pro(void) //TCP通讯
{
/*AGV的升降状态判断*/
if (IN_DownLimit && (IN_UpLimit==0))AGV.UDstatus=1;
else if (IN_UpLimit && (IN_DownLimit==0))AGV.UDstatus=3;
else AGV.UDstatus=2;
if (Tim5_TCP1Nums>90)//2min无法正常通讯，则判断通讯异常
{ VaIndexW_W(&NodeOnLine,13,0);}
else if(Tim5_SystemTimeNums>90)VaIndexW_W(&NodeOnLine,13,1);
/*通讯点动动作*/
if(Tim5_TCP1Nums>2)TCP_AcNo=0;//若TCP通讯断开则点动操作动作变为0
if( (TCP_AcNo==0)&&TCP_LastAcNo)
{ if(TCP_Mode==200)Action_Fc(TCP_LastAcNo);//200-对中;201-升到顶;202-降到底;
}
TCP_LastAcNo=TCP_AcNo;
}