Load.c
8 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
#include "hardware.h"
#include "all_value.h"
/*********************************************************************************
函数名：CanMc_W4_Message
功能：SDO轴写4B参数
参数：
返回：无返回。
实现方法：
编程：pcy
日期：2021-12-16
*********************************************************************************/
float AngCCD_Find(float*Ang)//根据当前车姿态角度获取对中摆正角度
{
if( ((*Ang)>315)|((*Ang)<=45) ) { return 0;}
else if( ((*Ang)>45) &&((*Ang)<=135) ){ return 90;}
else if( ((*Ang)>135)&&((*Ang)<=225) ) { return 180;}
else if( ((*Ang)>225)&&((*Ang)<=315) ) { return 270;}
else return -90;
}
//float Ang_CCD;
short X_D;
short Y_D;
float Ang_D;
u8 AdjustNums;
u8 Dx_Per;
u8 Dy_Per;
#define XYInDeltaCCDEn 1u//调节后x/y必须在范围内
#define XYInDeltaSlamEn 1u//调节后x/y必须在范围内
u8 CameraNo;
void Load_Pro(void)
{
signed char Dir;
float Ang_HelmTarget;
float Ang_Delta;
u8 Type2_AdLev;
switch(AGV.Load_Step)//Load_Step:1-50是CCD对中，51-89是SLAM对中
{
case 1://
AGV.AxisEn_Step=1;
AGV.Axis_No=0x3E;
AdjustNums=0;
AGV.Load_Step++;
break;
case 2://舵轮组都已使能
if (AGV.AxisEn_Step==100) AGV.Load_Step++;
break;
case 3://
if(AGV.Type2_AdLev>100)Type2_AdLev=AGV.Type2_AdLev-100;else Type2_AdLev=AGV.Type2_AdLev;
switch(Type2_AdLev)
{ //CCD对中精度属性 -11充电位普通对中,12料台下普通精确对中，13料台下高精度精确对中，CCD对中等级：数字越大级别高，等级高，精度高;
case 13://
Dx_Per=Dx13_Per;
Dy_Per=Dy13_Per;
Ang_MaxDelta=Da13_Per;//此处单位是*0.01度
break;
case 11://
Dx_Per=Dx11_Per;
Dy_Per=Dy11_Per;
Ang_MaxDelta=Da11_Per;
break;
case 12:
Dx_Per=Dx12_Per;
Dy_Per=Dy12_Per;
Ang_MaxDelta=Da12_Per;
break;
//对中SLAM精度属性 1充电位普通对中，2料台下普通精确对中，3料台下高精度精确对中，CCD对中等级：1-3，级别高，等级高，精度越高
case 3://
Dx_Per=Dx3_Per;
Dy_Per=Dy3_Per;
Ang_MaxDelta=Da3_Per;
break;
case 1://
Dx_Per=Dx1_Per;
Dy_Per=Dy1_Per;
Ang_MaxDelta=Da1_Per;
break;
case 2:
Dx_Per=Dx2_Per;
Dy_Per=Dy2_Per;
Ang_MaxDelta=Da2_Per;
break;
}
if(Type2_AdLev>10&&Type2_AdLev<14)AGV.Load_Step=10; /*CCD对中调节*/
else if(Type2_AdLev<4)AGV.Load_Step=60; /*Slam对中调节*/
break;
/*CCD对中调节*/
case 10://
AGV.Load_Step++;
break;
case 11://若是需要车本体以外的定位参考信息
if(AGV.Type2_AdLev>100){ VaIndexB_W(&AGV.OtheSta2,1,0);AGV.Load_Step++;}
else AGV.Load_Step=14;
break;
case 12://获取此刻料台外部偏差值
if (VaIndexB_R(&AGV.OtheSta2,1))
{
DeltaX_LTAdjust=TCP_DX_LTAdjust;
DeltaY_LTAdjust=TCP_DY_LTAdjust;
AGV.Load_Step=14;
}
break;
case 14://是否依赖上视觉定位再次判断
if(VaIndexB_R(&AGV.OtheSta1,4)&& VaIndexDW_R(&F_AgvContr,11)&&IN_DownLimit)//有上视觉且在低位
{ CameraNo=1;
CCD_M[1].Tim4_Usart56Nums=0;
AGV.Load_Step++;
}
else//只看下视觉
{ CameraNo=0;
AGV.Load_Step=20;
VaIndexB_W(&AGV.OtheSta1,4,0);
CampareAdjustPEn=1;
}
break;
case 15://观察是否有上视觉读取到信息
if(CCD_M[CameraNo].CodeOn)
{ AGV.Load_Step=20;}
else if(CCD_M[CameraNo].Tim4_Usart56Nums>CcdMissTime){ CameraNo=0;AGV.Load_Step=20;}
break;
case 20://
if(CameraNo==0)VaIndexB_W(&AGV.OtheSta1,2,0);
AGV.Load_Step++;
break;
case 21://根据相机位置，计算距离
VaIndexB_W(&AGV.OtheSta1,CameraNo,1);
if(CCD_M[CameraNo].CodeOn)
{
F_Un.A_CCD_T=CCD_M[CameraNo].Ang;
if( (abs(CCD_M[CameraNo].X)<=Dx_Per)&&(abs(CCD_M[CameraNo].Y)<=Dy_Per) )
{ AGV.Load_Step=30;}
else
{
if(CCD_M[CameraNo].X==0)Ang_HelmTarget=90;
else Ang_HelmTarget=atan((float)CCD_M[CameraNo].Y/(float)CCD_M[CameraNo].X)*Du_Pi;
if(abs(Ang_HelmTarget)<91)
{ Motor[2].PcToM.Angle_Target=Motor[4].PcToM.Angle_Target=Ang_HelmTarget;
if (CCD_M[CameraNo].X==0)//目标点在车体坐标系Y轴上
{ if(CCD_M[CameraNo].Y>0)Dir=1;else Dir=-1; }
else if (CCD_M[CameraNo].X>0) Dir=1;
else Dir=-1;
Motor[1].PcToM.Angle_Target=Motor[3].PcToM.Angle_Target=(float)Dir*(float)(sqrt( pow(CCD_M[CameraNo].X,2)+pow(CCD_M[CameraNo].Y,2) ))*0.1f;
AGV.Load_Step++;
}
}
}
break;
case 22://启动舵轮转走一定距离
AGV.GoDis_Step=1;
AGV.Load_Step++;
break;
case 23://判断是否完成
if(AGV.GoDis_Step==100)AGV.Load_Step++;
break;
case 24://读码时间清零
AGV.AllAct_Time5Nums=0;
AGV.Load_Step++;
break;
case 25://根据码的角度来调节
if((CCD_M[CameraNo].CodeOn)|(AGV.AllAct_Time5Nums>1))
{ if (CCD_M[CameraNo].CodeOn) F_Un.A_CCD_T=CCD_M[CameraNo].Ang;
AGV.Load_Step=30;
}
break;
case 30://旋转
F_Un.A_CCD_T=AngCCD_Find(&F_Un.A_CCD_T);
AGV.RotationCCD_Step=1;
AGV.Load_Step=35;
break;
case 35://判断是否完成车身旋转
// if( VaIndexB_R(&AGV.AllAxisOK,2) && (AGV.RotationCCD_Step==100) )
if(AGV.RotationCCD_Step==100)
{
AGV.Load_Step++;
AdjustNums++;}
break;
case 36://回到初始判断是否达到定位精度要求
if (XYInDeltaCCDEn)
{ VaIndexB_W(&AGV.OtheSta1,CameraNo,1);
if( CCD_M[CameraNo].CodeOn )
{ if( (abs(CCD_M[CameraNo].X)<=Dx_Per)&&(abs(CCD_M[CameraNo].Y)<=Dy_Per) )AGV.Load_Step=40;
else if(AdjustNums>1)AGV.Load_Step=40;
else AGV.Load_Step=21;
}
}
else AGV.Load_Step=40;
break;
case 40://
if( (CameraNo==0)&& VaIndexB_R(&AGV.OtheSta1,4) )
{ CameraNo=1;
AGV.Load_Step=21;
AdjustNums=0;
}
else AGV.Load_Step=90;
break;
/*SLam对中调节-终点要给目标角度（工位）*/
case 60://
VaIndexB_W(&SlamMess_Fresh,4,0);
AGV.Load_Step=63;
break;
case 63://计算舵轮各轴要执行的数据
if ( VaIndexB_R(&SlamMess_Fresh,4) && (Status_SlamLaser==2) )
{
Ang_Delta=AngDelta_Calculate(Ang_SlamAgv-90.0f,Ang_CalculateLine(AgvSP,EndPoint));
if( Ang_Delta<=90 && Ang_Delta>-90 )Dir=1;else Dir=-1;
Motor[1].PcToM.Angle_Target=Motor[3].PcToM.Angle_Target=Dis_CalculateLine(AgvSP,EndPoint)*(float)Dir;
Ang_HelmTarget=Ang_Delta;
if(Ang_Delta>90)Ang_HelmTarget-=180.0f;
else if(Ang_Delta<=-90)Ang_HelmTarget+=180.0f;
Motor[2].PcToM.Angle_Target=Motor[4].PcToM.Angle_Target=Ang_HelmTarget;//舵轮需要转到的目标角度
AGV.Load_Step++;
}
break;
case 64://启动舵轮转走一定距离
AGV.GoDis_Step=1;
AGV.Load_Step++;
break;
case 65://判断是否完成
if(AGV.GoDis_Step==100)
{ if(AGV.Line_Step==12)AGV.Load_Step=70;
else AGV.Load_Step++;
}
break;
case 66://判断指令中是否要修正角度
if(EndPoint.Ang==0)AGV.Load_Step=70;
else { AGV.Load_Step++;}
break;
case 67://启动SLAM角度调整
F_Un.A_Slam_T=(float)EndPoint.Ang*0.1f;
AGV.RotationSlam_Step=1;
AGV.Load_Step++;
break;
case 68://判断SLAM角度调整 是否完成
if(AGV.RotationSlam_Step==100)
{
VaIndexB_W(&SlamMess_Fresh,4,0);
AGV.Load_Step=70;
}
break;
case 70://判断是否达到定位精度要求
if (XYInDeltaSlamEn)
{
if ( VaIndexB_R(&SlamMess_Fresh,4) && (Status_SlamLaser==2) )
{ if( (fabs(AgvSP.X_Slam-EndPoint.X_Slam)<=Dx_Per)&&(fabs(AgvSP.Y_Slam-EndPoint.Y_Slam)<=Dy_Per) )AGV.Load_Step=90;
else if(AdjustNums>1)AGV.Load_Step=90;
else {AGV.Load_Step=60;AdjustNums++;}
}
}
else AGV.Load_Step=90;
break;
case 90://完成对中
AGV.Load_Step=100;
CampareAdjustPEn=0;
break;
case 100://完成对中
break;
}
}