AGV / agv_gelinmei · Files

#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;
}

float CalculatingDirectionAngle2(float startX,float startY,float endX,float endY)
{
	float Angle = 0;
	
	if(startX == endX)//垂直于X轴
	{
		if(endY > startY)
		{
			Angle = PI/2;
		}
		else if(endY < startY)
		{
			Angle = -PI/2;
		}
	}
	else//不垂直于X轴
		Angle = atan2((endY - startY) , (endX - startX));

	return Angle;
}

u8 CalculateCurDir(float A)
{
	static float dir = 0;
	if(A < 10 && A > -10)
	{
		dir = 1;
	}
	else if(A < 100 && A > 80)
	{
		dir = 2;
	}
	else if((A < -170 && A >= -180) || (A > 170 && A <= 180))
	{
		dir = 3;
	}
	else if(A < -80 && A > -100)
	{
		dir = -4;
	}
	return dir;
}
float CalculateCurAngle(float A)//计算当前处于多少度
{
	static float angle = 0;
	if(A < 10 && A > -10)
	{
		angle = 0;
	}
	else if(A < 100 && A > 80)
	{
		angle = 90;
	}
	else if((A < -170 && A >= -180) || (A > 170 && A <= 180))
	{
		angle = 180;
	}
	else if(A < -80 && A > -100)
	{
		angle = -90;
	}
	return angle;
}

u8 CalculateCurHeadDir(float A)//计算当前车头方向
{
	static u8 symbol = 0;
	if(A < 30 && A > -30)
	{
		symbol = 1;
	}
	else if(A < 120 && A > 60)
	{
		symbol = 2;
	}
	else if((A < -150 && A >= -180) || (A > 150 && A <= 180))
	{
		symbol = 3;
	}
	else if(A < -60 && A > -120)
	{
		symbol = 4;
	}
	return symbol;
}

//计算目标方向与当前车体方向的夹角 偏左大于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;
}

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 Y_DirFlag,int X_DirFlag)//目的点距离当前点方向，XY正方形，1为正，-1为负
{
	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(X_DirFlag == 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
	{
		TurnOffP.TarX = P->TarX;
		
		if(Y_DirFlag == 1)//沿Y正方向走 
		{
			TurnOffP.TarY = P->TarY+Dis;
		}
		else//沿Y负方向走 
			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 xDirFlag = 0,yDirFlag = 0;
	if(P.TarX > Point3.TarX)//往X轴负方向
		xDirFlag = -1;
	else
		xDirFlag = 1;
	
	if(P.TarY > Point3.TarY)//往y轴负方向
		yDirFlag = -1;
	else
		yDirFlag = 1;
	//分后退讨论
	if(agv.Command.CurDirection == 9 || agv.Command.CurDirection == 10)
	{
		P.CurAngle += PI;
		
		if(P.CurAngle > PI)
		{
			P.CurAngle -= 2*PI;
		}
		else if(P.CurAngle < -PI)
		{
			P.CurAngle += 2*PI;
		}
	}
	/***********************************/
	CoordinatePoints PTurnOff,PCROSS,Circle;//计算点位
	
	static Line L1,L2,L3,L4;
	
	L1 = getLine1(P);//以一个点构造直线
	
	L2 = getLine2(Point2,Point3);//两个点构造直线
	
	PCROSS = getCrossPoint(&L1,&L2);
	
	DisPB = TwoPointDistance(P,PCROSS);
	
	navi.Private.distancePB = DisPB;
	
	PTurnOff = getTurnOffPoint(&L2,&PCROSS,DisPB,yDirFlag,xDirFlag);//求出弯点

	L3 = getVerticalLine(&L1,&P);
	
	L4 = getVerticalLine(&L2,&PTurnOff);
	
	Circle = getCrossPoint(&L3,&L4);
	
	return Circle;
}

void CalculateDistance()
{
    navi.Public.DistanceAgvToSTART = TwoPointDistance(StartPoint, CurrentCenterPoint); //起始点到agv实际距离

    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); //起始点到目标点距离
	
//	static u8 firstSig = 1;
//	if(agv.Command.standSiteID == 1 && traffic_land_marksReal.size > 2&&(agv.Command.CurDirection == 1||agv.Command.CurDirection == 2))//刚开始启动计算到目标点距离
//	{
//		if(firstSig)
//		{
//			firstSig = 0;
//			navi.Public.LastDistanceAgvToTARGET = navi.Public.DistanceAgvToTARGET;
//		}
//		if(navi.Public.DistanceAgvToTARGET - navi.Public.LastDistanceAgvToTARGET > 100)
//			SetAlarm(0x0080);
//	}
//	else
//		firstSig = 1;
}

/********判断点位于向量的哪侧**********/
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);

//	  *Theta ;//+= palstance * timecount;
    *XBias += Dx;
    *YBias += Dy;
}
//void CalculateXBias(double Vx, double palstance, double timecount, float *YBias, float *Theta)
//{
//    double Time = timecount;
//    *Theta += palstance * timecount;
//    double Dy = Vx * Time * sin((*Theta));// + palstance * Time / 2);

//    *YBias += Dy;
//}