Calculation.h
1.89 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
#ifndef __CALCULATION_H
#define __CALCULATION_H
#include "bsp.h"
typedef struct
{
float TarX;
float TarY;
float CurAngle;
} CoordinatePoints; //模块局部结构体
/*定义用于计算的点
StartPoint TargetPoint直线运行当前点（起点）和下一个点（目标点）
CurrentCenterPoint agv运动中心实时坐标
PointOne,PointTwo,PointThree,PointFour弧线转弯用的四个点，上一个点，当前点，下一个点，下两个点
A,B为单机测试时，放置屏幕可以调整的坐标
*/
extern CoordinatePoints StartPoint, TargetPoint, CurrentCenterPoint, PointOne, PointTwo, PointThree, PointFour, CircleCenterPoint,FrontViewPoint;
typedef struct
{
CoordinatePoints p1,p2;//两点构成一条直线
int VerticalWithX;//1垂直于X轴 2垂直于Y轴 3不与坐标轴垂直
double k,b;//直线方程参数y = kx + b
}Line;
extern float mapping(float val, float I_Min, float I_Max, float O_Min, float O_Max);
extern float TwoPointDistance(CoordinatePoints PointOne, CoordinatePoints PointTwo);
extern CoordinatePoints GetCircleCenterPoint(CoordinatePoints P,CoordinatePoints Point2,CoordinatePoints Point3);//计算圆心
extern double CalCoordinateDis(CoordinatePoints CurrentCenterPoint, CoordinatePoints StartPoint, CoordinatePoints TargetPoint);
extern float CalculatingDirectionAngle(CoordinatePoints PointOne, CoordinatePoints PointTwo);
extern float CalculatingCurrentAndTargetAngle(float agvActualAngle, float agvTargetAngle);
extern void CalculateDistance();
extern int Point_Line_Pos(double x, double y, double startx, double starty, double endx, double endy);
//void CalculateXBias(double Vx, double palstance, double timecount, float *YBias, float *Theta);
void CalculateXBias(float Vx, float Vy, float palstance, float timecount, float *XBias, float *YBias, float *Theta);
extern CoordinatePoints getControlTargetPoint(CoordinatePoints Point,Line Lpath,float dis);
extern Line getLine1(CoordinatePoints Point);
extern Line getLine2(CoordinatePoints Point1,CoordinatePoints Point2);
#endif