Line.c
12.5 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
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
#include "hardware.h"
#include "all_value.h"
/*********************************************************************************
函数名：
功能：
参数：
返回：无返回。
实现方法：
编程：pcy
日期：2021-12-16
*********************************************************************************/
float OverrideAuto_Go;
float Override1Auto_Go=1.2f;
u8 Override2Auto_Go=2;
u8 OverrideMinAuto_Go=4;
u8 OverrideMaxAuto_Go=55;
float AutoRadius;
#define Dis0_Delta 3.5f
#define Dis1_Delta 4.0f
#define Dis2_Delta 60.0f
u8 F_OverrideGo_KZ=0 ;//空载速度 百分比
u8 F_OverrideGo_FZ=0 ;//负载速度 百分比
float R1_A=32000;
float R2_A=24000;
#define R1_Max 30000.0f
#define R1_Min 22000.0f
#define R2_Max 35000.0f
#define R2_Min 25000.0f
#define R3_Max 41000.0f
#define R3_Min 33000.0f
#define OverrideLow1TJ_Go 40.0f
#define OverrideLow2TJ_Go 50.0f
u8 Tim5_PDNums;//用于无按键操作下的计时
#define LowAutoSp 200.0f //mm/s 速度低于此值时不再减速
#define LowAutoSp2 100.0f //mm/s 速度低于此值时不再减速
#define DisMax_FZ 3000.0f //开始减速的最大距离
#define DisMax_KZ 1600.0f //开始减速的最大距离
#define DisMin 180.0f //线性调整的最小距离
#define AccTime_FZ 300.0f //车中心负载加速时间单位10ms
#define AccTime_KZ 200.0f //车中心空载加速时间单位10ms
#define DccTime_FZ 160.0f //车中心负载减速时间单位10ms
#define DccTime_KZ 50.0f //车中心空载减速时间单位10ms
u16 Tim4_AccDccNums;//加速时间间隔计数
#define Dx_Max 250 //允许最大偏差量
# define LR_SafeEn 1 //雷达安全保护-减速启用
void Line1_Pro(void)//获取工作模式
{
u8 i;
float f1;//临时速度变量
float DisMax;
float AccTime,DccTime;
float Ang_LineGo;//路线的角度
s8 LineDir;
switch(AGV.Line_Step)
{
case 1://舵轮组检查
AGV.AxisEn_Step=1;
AGV.Axis_No=0x3E;//外加6号输送轴或者移栽轴
AGV.Line_Step++;
FirstPosOk=0;
break;
case 2://舵轮组都已使能
if (AGV.AxisEn_Step==100)AGV.Line_Step=4;
break;
case 4://计算行走方向
if ( VaIndexB_R(&SlamMess_Fresh,4) && (Status_SlamLaser==2) )//
{ if( Dis_CalculateLine(AgvSP,EndPoint)<65) AGV.Line_Step=90;
else
{ if(VaIndexB_R(&AGV.OtheSta,2))
{
Ang_LineGo=AGV.RouteAngDir=Ang_CalculateLine(LastEndP,EndPoint);
StartPoint=LastEndP;
}
else
{
Ang_LineGo=AGV.RouteAngDir=Ang_CalculateLine(AgvSP,EndPoint);
StartPoint=AgvSP;
}
AGV.DirGo=DirGo_Calculate(Ang_SlamAgv,Ang_LineGo);
if (AGV.DirGo!=0)
{ if(Ceshi[18]==1)
{ F_Un.A_Slam_T=Ang_LineGo;
AGV.DirGo=2;
}
else//期望车角度
{ F_Un.A_Slam_T=Ang_LineGo-((float)AGV.DirGo-2)*90.0f;
AngDeal(&F_Un.A_Slam_T);
}
if( Dis_CalculateLine(AgvSP,EndPoint)>500)AGV.Line_Step++;
else AGV.Line_Step=11;
}
}
VaIndexB_W(&SlamMess_Fresh,4,0);
}
break;
case 5://
if ( ActionAutoEn|(ManuConfirm==1) )
{
if( ((AGV.RouteP[0].P_Type1==3)|(AGV.RouteP[0].P_Type1==4))&& //1->行进普通点；2->拐点；3-充电点；4->工位料台；5->进工位料台前点；25->拐点+料台前点；
(Dis_CalculateLine(AgvSP,EndPoint)<3000)
)Ang_MaxDelta=220;//*0.01度
else Ang_MaxDelta=500;//*0.01度
AGV.RotationSlam_Step=1;
AGV.Line_Step++;
ManuConfirm=0;
}
break;
case 6://完成旋转
if (AGV.RotationSlam_Step==100)
{
Motor[2].PcToM.AbsoluteHold_Step=Motor[4].PcToM.AbsoluteHold_Step=0;
AGV.Line_Step++;}
break;
case 7://行走轴置零
Motor[1].PcToM.SetZero_Step=Motor[3].PcToM.SetZero_Step=1;
AGV.Line_Step++;
break;
case 8://判断是否完成置零
if( (Motor[1].PcToM.SetZero_Step==100) && (Motor[3].PcToM.SetZero_Step==100) )
AGV.Line_Step=13;
break;
case 11://SLAM微调 考虑此处将行走轴置零
AGV.Load_Step=1;
AGV.Type2_AdLev=2;
AGV.Line_Step++;
VaIndexB_W(&AGV.OtheSta,3,0);
break;
case 12://微调完成
if (AGV.Load_Step==100)AGV.Line_Step=90;
break;
case 13:
for(i=1;i<5;i++)
{
Motor[i].PcToM.Acc=1000;
Motor[i].PcToM.Dcc=1000;
}
AGV.Line_Step++;
break;
case 14://舵轮调整
if( (AGV.DirGo==2)|(AGV.DirGo==4) )
{ Motor[2].PcToM.Angle_Target=Motor[4].PcToM.Angle_Target=90;}
else
{ Motor[2].PcToM.Angle_Target=Motor[4].PcToM.Angle_Target=0;}
Motor[2].PcToM.AbsoluteHold_Enable=Motor[4].PcToM.AbsoluteHold_Enable=1;
Motor[2].PcToM.AbsoluteHold_Step=Motor[4].PcToM.AbsoluteHold_Step=1;
AGV.Line_Step++;
break;
case 15://舵轮角度调到位
if ( (fabs(Motor[2].MToPc.Angle_Actual-Motor[2].PcToM.Angle_Target)<DeltaAngPerm_Helmzhuan)&&(Motor[2].PcToM.AbsoluteHold_Step==20) &&
(fabs(Motor[4].MToPc.Angle_Actual-Motor[4].PcToM.Angle_Target)<DeltaAngPerm_Helmzhuan)&&(Motor[4].PcToM.AbsoluteHold_Step==20) )
AGV.Line_Step++;
Tim4_AccDccNums=0;
break;
case 16://走
if (VaIndexB_R(&AGV.OtheSta,0))//有负载
{
if(F_OverrideGo_FZ>2&&F_OverrideGo_FZ<80)OverrideMaxAuto_Go=F_OverrideGo_FZ;
else OverrideMaxAuto_Go=30;
AccTime=AccTime_FZ;
DccTime=DccTime_FZ;
DisMax=DisMax_FZ;
}
else
{ if(F_OverrideGo_KZ>5&&F_OverrideGo_KZ<80)OverrideMaxAuto_Go=F_OverrideGo_KZ;
else OverrideMaxAuto_Go=40;
AccTime=AccTime_KZ;
DccTime=DccTime_KZ;
DisMax=DisMax_KZ;
}
if (Dis_GoBackLine>DisMax){ OverrideAuto_Go=(float)OverrideMaxAuto_Go;}
else if(Dis_GoBackLine>DisMin)
{ OverrideAuto_Go=((float)OverrideMaxAuto_Go-(float)OverrideMinAuto_Go)/(DisMax-DisMin)*(Dis_GoBackLine-DisMin)+(float)OverrideMinAuto_Go;}
else if (Dis_GoBackLine>50){ OverrideAuto_Go=(float)Override2Auto_Go;}
else { OverrideAuto_Go=Override1Auto_Go;}
if(fabs(DelX_HVP_Line)>Dx_Max){;}
else Tim5_PDNums=0;
if(Status_SlamLaser!=2)//置信度不高时减速
{ OverrideAuto_Go=OverrideAuto_Go*0.6f;
}
#if(LR_SafeEn==1)
if(LRadar_Far)//远处有障碍物且速度过大时减速
{
if( (OverrideAuto_Go*0.01f*Motor[1].Max_Speed)>LowAutoSp )OverrideAuto_Go=LowAutoSp*100.0f/Motor[1].Max_Speed;
}
if(LRadar_Middle)//中区域检测到再次减速
{ if((OverrideAuto_Go*0.01f*Motor[1].Max_Speed)>LowAutoSp2 )OverrideAuto_Go=LowAutoSp2*100.0f/Motor[1].Max_Speed;
DccTime=50;
}
#endif
if(VaIndexDW_R(&F_AgvContr,10)&& VaIndexB_R(&AGV.OtheSta,5)&&(VaIndexDW_R(&Touch_Contr,9)==0))
{ OverrideAuto_Go=0;Set_ErrID(39,1);}//滚筒限位触发
if(Tim5_Slam3Nums>1)OverrideAuto_Go=0;//置信度不高超时停止
if(Tim5_PDNums>2)//偏差过大超过2s则停止运行
{ OverrideAuto_Go=0;
Set_ErrID(46,1); //ErrID=46;行走过程横向偏差过大
}
/*减速过程调节速度*/
if ( abs(Motor[1].MToPc.speed)>(OverrideAuto_Go*0.01f*(float)Motor[1].Max_rpm+60) )
{
f1=fabs(Motor[1].MToPc.speed)*100.f/(float)Motor[1].Max_rpm-(float)Tim4_AccDccNums/DccTime*100.0f;
if( f1>=OverrideAuto_Go )
{
OverrideAuto_Go=f1;
}
}
/*加速过程调节速度*/
if ( (abs(Motor[1].MToPc.speed)+50) <(OverrideAuto_Go*0.01f*(float)Motor[1].Max_rpm) )
{
f1=fabs(Motor[1].MToPc.speed)*100.f/(float)Motor[1].Max_rpm+(float)Tim4_AccDccNums/AccTime*100.0f;
if( f1<1 )OverrideAuto_Go=1;
else if( f1<OverrideAuto_Go )OverrideAuto_Go=f1;
}
Tim4_AccDccNums=0;
/*根据速度和负载确定调节半径范围*/
if (abs(Motor[1].MToPc.speed)<(OverrideLow1TJ_Go*0.01f*(float)Motor[1].Max_rpm) )
{ R1_A=R1_Max;
R2_A=R1_Min;
}
else if (abs(Motor[1].MToPc.speed)<(OverrideLow2TJ_Go*0.01f*(float)Motor[1].Max_rpm) )
{ if (VaIndexB_R(&AGV.OtheSta,0))
{ R1_A=R3_Max;
R2_A=R3_Min; }
else
{ R1_A=R2_Max;
R2_A=R2_Min;}
}
else
{ R1_A=R3_Max;
R2_A=R3_Min;
}
/*根据点偏差确定弧半径*/
if (fabs(DelX_HVP_Line)<Dis0_Delta)
{
if(AGV.DirGo==1)Fc_Matrix_Agv(OverrideAuto_Go*0.01f*Motor[1].Max_Speed,0,0,0,0,0);
else if(AGV.DirGo==2)Fc_Matrix_Agv(0,OverrideAuto_Go*0.01f*Motor[1].Max_Speed,0,0,0,0);
else if(AGV.DirGo==3)Fc_Matrix_Agv(-OverrideAuto_Go*0.01f*Motor[1].Max_Speed,0,0,0,0,0);
else if(AGV.DirGo==4)Fc_Matrix_Agv(0,-OverrideAuto_Go*0.01f*Motor[1].Max_Speed,0,0,0,0);
}
else
{ if (DelX_HVP_Line>0)LineDir=-1;
else LineDir=1;
if (fabs(DelX_HVP_Line)<Dis1_Delta)AutoRadius=R1_A;
else if (fabs(DelX_HVP_Line)<Dis2_Delta)AutoRadius=(fabs(DelX_HVP_Line)-Dis1_Delta)/(Dis2_Delta-Dis1_Delta)*(R2_A-R1_A)+R1_A;
else AutoRadius=R2_A;
if(OverrideAuto_Go==0){LineDir=0;AutoRadius=0;}
if(AGV.DirGo==1)Fc_Matrix_Agv(OverrideAuto_Go*0.01f*Motor[1].Max_Speed,0,(float)LineDir,AutoRadius,0,0);
else if(AGV.DirGo==2)Fc_Matrix_Agv(0,OverrideAuto_Go*0.01f*Motor[1].Max_Speed,(float)LineDir,AutoRadius,0,0);
else if(AGV.DirGo==3)Fc_Matrix_Agv(-OverrideAuto_Go*0.01f*Motor[1].Max_Speed,0,(float)LineDir,AutoRadius,0,0);
else if(AGV.DirGo==4)Fc_Matrix_Agv(0,-OverrideAuto_Go*0.01f*Motor[1].Max_Speed,(float)LineDir,AutoRadius,0,0);
}
if (Dis_GoBackLine<4)
{ Fc_Matrix_Agv(0,0,0,0,0,0);
Motor[2].PcToM.AbsoluteHold_Step=Motor[4].PcToM.AbsoluteHold_Step=0;
Motor[2].PcToM.AbsoluteHold_Enable=Motor[4].PcToM.AbsoluteHold_Enable=0;
Motor[2].PcToM.TargetSpeed=Motor[4].PcToM.TargetSpeed=0;
AGV.Line_Step++;
}
//
if (((AGV.TaskManu_Step==0)|(AGV.TaskManu_Step==100)) )//
{
if (AGV.RouteP[1].Code )//判断是否需要更新目标点
{
if(AGV.RouteP[0].SP.Ang|AGV.RouteP[0].P_AD_Type2|(AGV.RouteP[0].P_AC_Type3>1)){;}//把当前点走完
else
{
Ang_LineGo=AngDelta_Calculate(AGV.RouteAngDir,Ang_CalculateLine(AGV.RouteP[0].SP,AGV.RouteP[1].SP));
if( (fabs(Ang_LineGo)<AngLineDeltaMax)|
((fabs(Ang_LineGo)<=AngLineDuanDeltaMax)&& Dis_CalculateLine(AGV.RouteP[0].SP,AGV.RouteP[1].SP)<=400)//行驶方向角度变化不大
)
{
// StartPoint=AGV.RouteP[0].SP;
RouteGoAcPMiss_fc();
EndPoint=AGV.RouteP[0].SP;
AGV.RouteAngDir=Ang_CalculateLine(StartPoint,AGV.RouteP[0].SP);
}
}
}
}
if (AGV.RouteP[0].Code )//判断完成状态是否需要清除
{
if(VaIndexB_R(&AGV.OtheSta,3) ){ if(Dis_GoBackLine<400)VaIndexB_W(&AGV.OtheSta,3,0);}
else if(Dis_GoBackLine>400)
{ for(i=0;i<4;i++)
{ if(TaskP[i].Code==AGV.VirCode)
{VaIndexB_W(&AGV.OtheSta,3,1);break;}
}
}
}
break;
case 17://
AGV.Line_Step=90;
break;
case 90://走完
VaIndexB_W(&AGV.OtheSta,2,1);
LastEndP=EndPoint;
AGV.Line_Step=100;
break;
case 100://
break;
}
if(AGV.Line_Step>0&&AGV.Line_Step<100)HeadRun_PoCal_Pro();
}
#define Dis_HVP_AgvC 1200.0f//纠偏时车头点到车中心的设定距离
float Dis_GoBackLine;//剩余还未走的距离
float DelX_HVP_Line;//行驶偏差
void HeadRun_PoCal_Pro(void)//行驶偏差计算函数
{
float Dis_GoDoneLine;//已走距离
float Ang_HVP_S;
float Ang_SH_Line;
float Ang_SE_Line;
float Ang_HSE_Line;
float Ang_SC_Line;
float Ang_CSE_Line;
Str_SlamPoint HVPoint;
if ( VaIndexB_R(&SlamMess_Fresh,4) && (Status_SlamLaser==2) )
{
Ang_HVP_S=((float)AGV.DirGo-2)*90.0f+Ang_SlamAgv;
AngDeal(&Ang_HVP_S);
HVPoint.X_Slam=AgvSP.X_Slam+Dis_HVP_AgvC*cos(Ang_HVP_S*Pi_Du);
HVPoint.Y_Slam=AgvSP.Y_Slam+Dis_HVP_AgvC*sin(Ang_HVP_S*Pi_Du);
Ang_SH_Line=Ang_CalculateLine(StartPoint,HVPoint);
Ang_SE_Line=Ang_CalculateLine(StartPoint,EndPoint);
Ang_HSE_Line=AngDelta_Calculate(Ang_SE_Line,Ang_SH_Line);
DelX_HVP_Line=Dis_CalculateLine(StartPoint,HVPoint)*sin(Ang_HSE_Line*Pi_Du);//负值为在左侧
/*已走的距离+剩余距离*/
Ang_SC_Line=Ang_CalculateLine(StartPoint,AgvSP);
Ang_CSE_Line=AngDelta_Calculate(Ang_SE_Line,Ang_SC_Line);
Dis_GoDoneLine=Dis_CalculateLine(StartPoint,AgvSP)*cos(Ang_CSE_Line*Pi_Du);
Dis_GoBackLine=Dis_CalculateLine(StartPoint,EndPoint)-Dis_GoDoneLine;
VaIndexB_W(&SlamMess_Fresh,4,0);
}
}
u8 DirGo_Calculate(float Ang_C,float Ang_Go)//车行驶方向选择函数
{
float AngDelta;
u8 Dir;
u8 i;
float AngDir;
Dir=0;
for(i=1;i<5;i++)
{ AngDir=((float)i-2)*90.0f+Ang_C;
AngDeal(&AngDir);
AngDelta=AngDelta_Calculate(Ang_Go,AngDir);
if(fabs(AngDelta)<=45) { Dir=i;break; }
}
return Dir;
}