Calculation.c
11.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
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
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
#include "Calculation.h"
float mapping(float val, float I_Min, float I_Max, float O_Min, float O_Max)
{
val = val < I_Min ? I_Min : val;
val = val > I_Max ? I_Max : val;
return ((val - I_Min) * (O_Max - O_Min) / (I_Max - I_Min)) + O_Min;
}
float CalculatingDirectionAngle(CoordinatePoints PointOne, CoordinatePoints PointTwo)
{
float Angle = 0;
if(PointOne.TarX == PointTwo.TarX)//垂直于X轴
{
if(PointTwo.TarY > PointOne.TarY)
{
Angle = PI/2;
}
else if(PointTwo.TarY < PointOne.TarY)
{
Angle = -PI/2;
}
}
else//不垂直于X轴
Angle = atan2((PointTwo.TarY - PointOne.TarY) , (PointTwo.TarX - PointOne.TarX));
return Angle;
}
//计算目标方向与当前车体方向的夹角 偏左大于0 偏右 小于0
float CalculatingCurrentAndTargetAngle(float agvActualAngle, float agvTargetAngle)
{
float IncludedAngle = 0;
IncludedAngle = agvActualAngle - agvTargetAngle;
if (IncludedAngle <= -PI)
IncludedAngle += 2.0f * PI;
if (IncludedAngle >= PI)
IncludedAngle -= 2.0f * PI;
return IncludedAngle;
}
//两点距离公式
float TwoPointDistance(CoordinatePoints PointOne, CoordinatePoints PointTwo)
{
return sqrt(pow((PointOne.TarX - PointTwo.TarX), 2.0f) + pow((PointOne.TarY - PointTwo.TarY), 2.0f));
}
Line getLine1(CoordinatePoints Point)//一点生成直线
{
Line L;
L.p1 = Point;
if(fabs(fabs(Point.CurAngle) - PI/2) < 0.001)//垂直于X轴
{
L.VerticalWithX = 1;
}
else //不垂直于X轴
{
L.VerticalWithX = 0;
L.k = tan(Point.CurAngle);
L.b = Point.TarY - Point.TarX*L.k;
}
return L;
}
Line getLine2(CoordinatePoints Point1,CoordinatePoints Point2)//两点生成直线
{
Line L;
L.p1 = Point1;
L.p2 = Point2;
if(L.p1.TarX == L.p2.TarX)//垂直于X轴
{
L.VerticalWithX = 1;
}
else
{
L.VerticalWithX = 0;
L.k = (L.p1.TarY - L.p2.TarY)/(L.p1.TarX - L.p2.TarX);
L.b = L.p1.TarY - L.p1.TarX*L.k;
}
return L;
}
//void getLine(Line *L,int PointNum)//第二个参数,生成直线方式,1为一个点加斜率,2为两个点坐标
//{
// if(PointNum == 1)
// {
// if(fabs(fabs(L->p1.CurAngle) - PI/2) < 0.001)//垂直于X轴
// {
// L->VerticalWithX = 1;
// }
// else //不垂直于X轴
// {
// L->VerticalWithX = 0;
// L->k = tan(L->p1.CurAngle);
// L->b = L->p1.TarY - L->p1.TarX*L->k;
// }
// }
// else if(PointNum == 2)
// {
// if(L->p1.TarX == L->p2.TarX)//垂直于X轴
// {
// L->VerticalWithX = 1;
// }
// else
// {
// L->VerticalWithX = 0;
// L->k = (L->p1.TarY - L->p2.TarY)/(L->p1.TarX - L->p2.TarX);
// L->b = L->p1.TarY - L->p1.TarX*L->k;
// }
// }
//
//}
Line getVerticalLine(Line *L,CoordinatePoints *P)//过P点垂直于L的直线
{
Line VerticalLine;
VerticalLine.p1 = *P;
if(L->VerticalWithX == 1)
{
VerticalLine.VerticalWithX = 0;
VerticalLine.k = 0;
VerticalLine.b = P->TarY;
}
else
{
if(L->k == 0)
{
VerticalLine.VerticalWithX = 1;
}
else
{
VerticalLine.VerticalWithX = 0;
VerticalLine.k = -1/L->k;
VerticalLine.b = VerticalLine.p1.TarY - VerticalLine.k*VerticalLine.p1.TarX;
}
}
return VerticalLine;
}
/*求两条直线交点*/
CoordinatePoints getCrossPoint(Line *L1,Line *L2)
{
CoordinatePoints PCROSS;
if(L1->VerticalWithX == 1)
{
PCROSS.TarX = L1->p1.TarX;
PCROSS.TarY = L2->k*PCROSS.TarX + L2->b;
}
else if(L2->VerticalWithX == 1)
{
PCROSS.TarX = L2->p1.TarX;
PCROSS.TarY = L1->k*PCROSS.TarX + L1->b;
}
else
{
if(L1->k == 0)//直线一垂直于Y轴
{
PCROSS.TarY = L1->p1.TarY;
PCROSS.TarX = (PCROSS.TarY - L2->b)/L2->k;
}
else if(L2->k == 0)//直线2垂直于Y轴
{
PCROSS.TarY = L2->p1.TarY;
PCROSS.TarX = (PCROSS.TarY - L1->b)/L1->k;
}
else
{
PCROSS.TarX = (L2->b - L1->b)/(L1->k - L2->k);
PCROSS.TarY = PCROSS.TarX*L1->k + L1->b;
}
}
return PCROSS;
}
//根据当前点和已知路径直线,以及预瞄距离,求预瞄点
CoordinatePoints getControlTargetPoint(CoordinatePoints Point,Line Lpath,float dis)
{
CoordinatePoints Pfoot;
Line Lfoot;//垂线
Lfoot = getVerticalLine(&Lpath,&Point);
Pfoot = getCrossPoint(&Lpath,&Lfoot);//垂足
CoordinatePoints valuePoint;
if(Lpath.VerticalWithX == 1)//垂直于X轴
{
valuePoint.TarX = Pfoot.TarX;
if(TargetPoint.TarY > StartPoint.TarY)
valuePoint.TarY = Pfoot.TarY + dis;
else
valuePoint.TarY = Pfoot.TarY - dis;
}
else//不垂直于X轴
{
if(Lpath.k > 0)
{
if(TargetPoint.TarY > StartPoint.TarY)
{
valuePoint.TarX = Pfoot.TarX + dis*cos(atan(Lpath.k));
valuePoint.TarY = Pfoot.TarY + dis*sin(atan(Lpath.k));
}
else
{
valuePoint.TarX = Pfoot.TarX - dis*cos(atan(Lpath.k));
valuePoint.TarY = Pfoot.TarY - dis*sin(atan(Lpath.k));
}
}
else if(Lpath.k < 0)
{
if(TargetPoint.TarY > StartPoint.TarY)
{
valuePoint.TarX = Pfoot.TarX - dis*cos(atan(-Lpath.k));
valuePoint.TarY = Pfoot.TarY + dis*sin(atan(-Lpath.k));
}
else
{
valuePoint.TarX = Pfoot.TarX + dis*cos(atan(-Lpath.k));
valuePoint.TarY = Pfoot.TarY - dis*sin(atan(-Lpath.k));
}
}
else
{
valuePoint.TarY = Pfoot.TarY;
if(TargetPoint.TarX > StartPoint.TarX)
{
valuePoint.TarX = Pfoot.TarX + dis;
}
else
{
valuePoint.TarX = Pfoot.TarX - dis;
}
}
}
return valuePoint;
}
/*求动态出弯点*/
//1,(y-y0)^2+(x-x0)^2=L^2;2,y=kx+b;
// 一元二次方程Ax^2+Bx+C=0中,
// 一元二次方程求根公式:
// 两根x1,x2= [-B±√(B^2-4AC)]/2A
// ①(y-y0)^2+(x-x0)^2=L^2;
// ②y=kx+b;
// 由①②表达式得到:(k^2+1)x^2+2[(b-y0)k-x0]x+[(b-y0)^2+x0^2-L^2]=0
CoordinatePoints getTurnOffPoint(Line *L,CoordinatePoints *P,float Dis,int TurnFlag,int DirFlag)
{
CoordinatePoints TurnOffP;
if(L->VerticalWithX == 0)
{
double A =pow(L->k, 2) + 1;// A=k^2+1;
double B = 2 * ((L->b - P->TarY) * L->k - P->TarX);// B=2[(b-y0)k-x0];
// C=(b-y0)^2+x0^2-L^2
double C = pow(L->b - P->TarY, 2) + pow(P->TarX, 2) - pow(Dis, 2);
// 两根x1,x2= [-B±√(B^2-4AC)]/2A
double x1 = (-B + sqrt(pow(B, 2) - 4 * A * C)) / (2 * A);
double x2 = (-B - sqrt(pow(B, 2) - 4 * A * C)) / (2 * A);
if(TurnFlag == 1)
{
TurnOffP.TarX = x1;
TurnOffP.TarY = L->k*TurnOffP.TarX + L->b;
}
else
{
TurnOffP.TarX = x2;
TurnOffP.TarY = L->k*TurnOffP.TarX + L->b;
}
}
else
{
if(TurnFlag == 1)//右转
{
TurnOffP.TarX = P->TarX;
if(DirFlag == -1)//沿X负方向走
{
TurnOffP.TarY = P->TarY+Dis;
}
else//沿X正方形走
TurnOffP.TarY = P->TarY-Dis;
}
else//左转
{
TurnOffP.TarX = P->TarX;
if(DirFlag == -1)//沿X负方向走
{
TurnOffP.TarY = P->TarY-Dis;
}
else//沿X正方形走
TurnOffP.TarY = P->TarY+Dis;
}
}
return TurnOffP;
}
/*车体实时位置,路径B点C点*/
CoordinatePoints GetCircleCenterPoint(CoordinatePoints P,CoordinatePoints Point2,CoordinatePoints Point3)
{
static float DisPB = 0;
/*当沿X轴方向前进转弯时用,-1代表往X轴负方向,1代表往X轴正方向*/
static int DirFlag = -1;
if(P.TarX > Point2.TarX)//往X轴负方向
DirFlag = -1;
else
DirFlag = 1;
/***********************************/
CoordinatePoints PTurnOff,PCROSS,Circle;//计算点位
Line L1,L2,L3,L4;
L1 = getLine1(P);//以一个点构造直线
L2 = getLine2(Point2,Point3);//两个点构造直线
PCROSS = getCrossPoint(&L1,&L2);
DisPB = TwoPointDistance(P,PCROSS);
if(agv.Command.CurDirection == 8 || agv.Command.CurDirection == 10)//右转
{
PTurnOff = getTurnOffPoint(&L2,&PCROSS,DisPB,1,DirFlag);
}
else if(agv.Command.CurDirection == 7 || agv.Command.CurDirection == 9)
{
PTurnOff = getTurnOffPoint(&L2,&PCROSS,DisPB,0,DirFlag);
}
L3 = getVerticalLine(&L1,&P);
L4 = getVerticalLine(&L2,&PTurnOff);
Circle = getCrossPoint(&L3,&L4);
return Circle;
}
//计算得到直线方程的abc值,ax+by+c=0
//void GetLinePara(Line *L)
//{
// L->a = L->p1.TarY - L->p2.TarY;
// L->b = L->p2.TarX - L->p1.TarX;
// L->c = L->p1.TarX * L->p2.TarY - L->p2.TarX * L->p1.TarY;
//}
//void GetCircleCenterPoint(CoordinatePoints Point1, CoordinatePoints Point2, CoordinatePoints Point3, CoordinatePoints Point4)
//{
// Line L1, L2, L3, L4;
// L1.p1.TarX = PointOne.TarX;
// L1.p1.TarY = PointOne.TarY;
// L1.p2.TarX = PointTwo.TarX;
// L1.p2.TarY = PointTwo.TarY;
// GetLinePara(&L1); //得到经过点PointOne,PointTwo直线1
// L2.p1.TarX = PointThree.TarX;
// L2.p1.TarY = PointThree.TarY;
// L2.p2.TarX = PointFour.TarX;
// L2.p2.TarY = PointFour.TarY;
// GetLinePara(&L2); //得到经过点PointThree,PointFour直线2
// CoordinatePoints P, PPP, J, CirclePoint;
// //解L1,L2方程组得到交点P坐标
// double ddd = L1.a * L2.b - L2.a * L1.b;
// P.TarX = (L1.b * L2.c - L2.b * L1.c) / ddd;
// P.TarY = (L1.c * L2.a - L2.c * L1.a) / ddd;
// //根据向量比例关系得到出弯点坐标J点
// float disPC = 0, disPB = 0, k = 0;
// disPC = TwoPointDistance(PointThree, P);
// disPB = TwoPointDistance(PointTwo, P);
// k = disPB / disPC;
// J.TarX = (PointThree.TarX - P.TarX) * k + P.TarX;
// J.TarY = (PointThree.TarY - P.TarY) * k + P.TarY;
// //得到经过点PointTwo垂直于L1的直线方程L3
// L3.a = L1.b;
// L3.b = -L1.a;
// L3.c = -(L3.a * PointTwo.TarX + L3.b * PointTwo.TarY);
// //得到经过出弯点J垂直于L2的直线方程
// L4.a = L2.b;
// L4.b = -L2.a;
// L4.c = -(L4.a * J.TarX + L4.b * J.TarY);
// //L3和L4的交点即为圆心坐标
// double DDD = L3.a * L4.b - L4.a * L3.b;
// CircleCenterPoint.TarX = (L3.b * L4.c - L4.b * L3.c) / DDD;
// CircleCenterPoint.TarY = (L3.c * L4.a - L4.c * L3.a) / DDD;
//}
void CalculateDistance()
{
navi.Public.DistanceAgvToSTART = TwoPointDistance(StartPoint, CurrentCenterPoint); //起始点到agv实际距离
navi.Public.CenterOffset = -CalCoordinateDis(CurrentCenterPoint, StartPoint, TargetPoint);
navi.Public.VerticalDistanceAgvToSTART =
sqrt(pow(navi.Public.DistanceAgvToSTART, 2.0f) - pow(navi.Public.CenterOffset, 2.0f)); //起始点到agv垂直距离
navi.Public.DistanceAgvToTARGET = TwoPointDistance(CurrentCenterPoint, TargetPoint); //agv到目标点实际距离
navi.Public.VerticalDistanceAgvToTARGET =
sqrt(pow(navi.Public.DistanceAgvToTARGET, 2.0f) - pow(navi.Public.CenterOffset, 2.0f)); //agv到目标点垂直距离
navi.Public.DistanceSTARTtoTARGET = TwoPointDistance(StartPoint, TargetPoint); //起始点到目标点距离
}
/********判断点位于向量的哪侧**********/
int Point_Line_Pos(double x, double y, double startx, double starty, double endx, double endy)
{
double m;
float vx, vy;
vx = x - startx;
vy = y - starty;
m = (endx - startx) * vy - (endy - starty) * vx;
if (m > 0) //左侧
return -1;
else if (m < 0) //右侧
return 1;
else //共线
return 0;
}
// 计算点到边的距离
double CalCoordinateDis(CoordinatePoints CurrentCenterPoint, CoordinatePoints StartPoint, CoordinatePoints TargetPoint)
{
double x, y, startx, starty, endx, endy;
double h, s, p, sum, a, b, c;
x = CurrentCenterPoint.TarX;
y = CurrentCenterPoint.TarY;
startx = StartPoint.TarX;
starty = StartPoint.TarY;
endx = TargetPoint.TarX;
endy = TargetPoint.TarY;
sum = pow((x - startx), 2) + pow((y - starty), 2);
a = sqrt(sum);
sum = pow((x - endx), 2) + pow((y - endy), 2);
b = sqrt(sum);
sum = pow((startx - endx), 2) + pow((starty - endy), 2);
c = sqrt(sum);
p = (a + b + c) / 2;
sum = p * (p - a) * (p - b) * (p - c);
s = sqrt(sum);
h = (2 / c) * s;
int direction = Point_Line_Pos(x, y, startx, starty, endx, endy);
return h * direction;
}
//位置积分
void CalculateXBias(float Vx, float Vy, float palstance, float timecount, float *XBias, float *YBias, float *Theta)
{
static float Angle = 0;
switch (agv.Command.RunState)
{
case 0:
Angle = *Theta;
break;
case 1:
Angle = *Theta - PI/2;
break;
case 2:
Angle = *Theta - PI;
break;
case 3:
Angle = *Theta + PI/2;
break;
default:
break;
}
double Time = timecount;
double Dx = Vx * Time * cos((Angle) + palstance * Time / 2) - Vy * Time * sin((Angle) + palstance * Time / 2);
double Dy = Vx * Time * sin((Angle) + palstance * Time / 2) + Vy * Time * cos((Angle) + palstance * Time / 2);
*XBias += Dx;
*YBias += Dy;
}