LaRadar.c
4.98 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
#include "includes.h"
#include "hardware.h"
#include "all_value.h"
u8 Area_LRadarB; //后雷达选择区域
u8 Area_LRadarF; //前雷达选择区域
u8 LRadar_Near; //Scan近区域触发
u8 LRadar_Middle; //Scan中区域触发
u8 LRadar_Far; //Scan远区域触发
u8 LRadar_Ok; //
u8 LRadar_Choice; //1是前雷达，2是后雷达，3前后双雷达
u8 F_LaPic[8];//防撞图
void LaRadarB_AreaChoice(void)//后雷达根据选择的区域确定IO口输出
{
Out_LRadarBIn1=VaIndexB_R(&Area_LRadarB,0);
Out_LRadarBIn2=VaIndexB_R(&Area_LRadarB,1);
Out_LRadarBIn3=VaIndexB_R(&Area_LRadarB,2);
Out_LRadarBIn4=VaIndexB_R(&Area_LRadarB,3);
}
void LaRadar_Choice(u8 LRadarNo,u8 AralNo)//雷达图形选择函数
{
LRadar_Choice=LRadarNo;
if(LRadarNo==1) Area_LRadarF=AralNo-1;
else if(LRadarNo==2) Area_LRadarB=AralNo-1;
}
void LaRadar_Force(u8 Va)//
{
LRadar_Near=LRadar_Middle=LRadar_Far=LRadar_Ok=Va;
}
#define SafObstacleTy 2
void LaRadar_Pro(void)//
{
#if(SafObstacleTy==2)
u8 LaObJG;//区域间隔,数组存储内部间隔
u8 LaObJG2;//外部间隔
u8 LaNo;
#endif
if((SysKey==89)&&(VaIndexDW_R(&Touch_Contr,15))){;}
else
{
#if(SafObstacleTy==1)
switch(AGV.DirGo)
{
case 1: //Vx+
LaRadar_Choice(2,1);//后雷达1号图
break;
case 2: //Vy+
LaRadar_Choice(1,1);//前雷达1号图
break;
case 3: //Vx-
LaRadar_Choice(1,2);//前雷达2号图
break;
case 4: //Vy-
LaRadar_Choice(2,2);//后雷达2号图
break;
case 100: //旋转
LRadar_Choice=3;//双雷达都开，使用3号图
Area_LRadarF=2;
Area_LRadarB=2;
break;
}
#elif(SafObstacleTy==2)
if( VaIndexDW_R(&F_AgvContr,14) )//0-依赖高低位判断是否有负载；1-依赖货物检测
{ if(AGV.DirGo)
{ if(VaIndexDW_R(&F_AgvContr,8)&&(VaIndexDW_R(&F_AgvContr,9)==0) ) //仅轴0输送 下层输送
{ IN_Cargo1=0;}
else if(VaIndexDW_R(&F_AgvContr,8)==0) //无轴0输送 无下层输送
{ IN_Cargo2=0;}
}
if(IN_Cargo1|IN_Cargo2) VaIndexB_W(&AGV.OtheSta,0,1);//此处考虑是否要滤抖动
else VaIndexB_W(&AGV.OtheSta,0,0);
}
else
{ if(AGV.UDstatus==1)VaIndexB_W(&AGV.OtheSta,0,0);//低位，定性为无货
else VaIndexB_W(&AGV.OtheSta,0,1);//非低位认为有负载
}
if( VaIndexB_R(&AGV.OtheSta,0))LaObJG=4;//有负载
else { LaObJG=0;}
switch(AGV.DirGo)
{
case 1: //Vx+
LaObJG2=0;
LaNo=2;//后雷达
break;
case 2: //Vy+
LaObJG2=0;
LaNo=1;//前雷达
break;
case 3: //Vx-
LaObJG2=8;
LaNo=1;//前雷达
break;
case 4: //Vy-
LaObJG2=8;
LaNo=2;//后雷达
break;
case 100: //旋转
LRadar_Choice=3;//双雷达都开，使用3号图
Area_LRadarF=8;
Area_LRadarB=8;
break;
}
if(AGV.DirGo>0&&AGV.DirGo<5)
{ if(AGV.Line_Step==12){ LaRadar_Choice(LaNo,F_LaPic[2+LaObJG]+LaObJG2);}
else
{ if( (AGV.EndP_Type1==1)| //1->行进普通点；2->拐点；3-充电点；4->工位料台；5->进工位料台前点；25->拐点+料台前点；
((AGV.EndP_Type1==2)|(AGV.EndP_Type1==5)|(AGV.EndP_Type1==25))
)
{
if((AGV.EndP_Type1==1)|(Dis_GoBackLine>1500))LaRadar_Choice(LaNo,F_LaPic[LaObJG]+LaObJG2);//停止区域较长。后雷达1号图，8个为一个方向。8个里面根据负载有无确定，无负载0-3，有负载4-7
else { LaRadar_Choice(LaNo,F_LaPic[1+LaObJG]+LaObJG2);}//停止区域较短。后雷达2号图
}
else if(AGV.EndP_Type1==4){ LaRadar_Choice(LaNo,F_LaPic[2+LaObJG]+LaObJG2);}//
else if(AGV.EndP_Type1==3){ LaRadar_Choice(LaNo,F_LaPic[3]+LaObJG2);}//
else LaRadar_Choice(LaNo,F_LaPic[LaObJG]+LaObJG2);//上述都不满足开最大停止区域图
}
}
#endif
}
LaRadarB_AreaChoice();
switch(LRadar_Choice)
{
case 1: //1是前雷达
if(VaIndexDW_R(&F_AgvContr,3)&&(VaIndexDW_R(&Touch_Contr,9)==0))
{ LRadar_Near=IN_LRadarF_Near;//有物体进入是1
LRadar_Middle=IN_LRadarF_Middle;
LRadar_Far=IN_LRadarF_Far;
LRadar_Ok=IN_LRadarF_Ok;
}
else{ LaRadar_Force(0);}
break;
case 2: //2是后雷达
if(VaIndexDW_R(&F_AgvContr,4)&&(VaIndexDW_R(&Touch_Contr,9)==0))
{ LRadar_Ok=IN_LRadarB_Ok;
LRadar_Near=IN_LRadarB_Near;//有物体进入是1
LRadar_Middle=IN_LRadarB_Middle;
LRadar_Far=IN_LRadarB_Far;
}
else{ LaRadar_Force(0);}
break;
case 3: //前雷达+后雷达
if(VaIndexDW_R(&F_AgvContr,3)&&(VaIndexDW_R(&Touch_Contr,9)==0))
{ LRadar_Ok=IN_LRadarF_Ok;
LRadar_Near=IN_LRadarF_Near;//有物体进入是1
LRadar_Middle=IN_LRadarF_Middle;
LRadar_Far=IN_LRadarF_Far;
}
else{ LaRadar_Force(0);}
if(VaIndexDW_R(&F_AgvContr,4)&&(VaIndexDW_R(&Touch_Contr,9)==0))
{ LRadar_Ok=(LRadar_Ok&&IN_LRadarB_Ok);
LRadar_Near=(IN_LRadarB_Near| LRadar_Near);//有物体进入是1
LRadar_Middle=(IN_LRadarB_Middle| LRadar_Middle);
LRadar_Far=(IN_LRadarB_Far| LRadar_Far);
}
break;
default://不选模式取前雷达的结果
break;
}
}