导航车体驱动代码

#include<math.h>


/*控制器，俯视建立坐标系*/


//控制参数
#define KpOffset 1 // 侧偏比例控制参数
#define KiOffset 1 // 侧偏积分控制参数
#define KpPsi 1 // 偏航比例控制参数
#define KiPsi 1 // 偏航积分控制参数
#define StopLimit 0.5 // 到位停止门限（m）
#define StopPsiCtrlLimit 1 // 偏航角到位停止门限, （°）
#define CtrlValueOffset_IMin -5 // 侧偏控制量积分限幅最小值
#define CtrlValueOffset_IMax 5 // 侧偏控制量积分限幅最大值
#define CtrlValuePsi_IMin -5 // 偏航控制量积分限幅最小值
#define CtrlValuePsi_IMax 5 // 偏航控制量积分限幅最大值
#define CtrlValueOffset_Min -30 // 侧偏控制量限幅最小值
#define CtrlValueOffset_Max 22.57 // 侧偏控制量限幅最大值
#define CtrlValuePsi_Min -30 // 偏航控制量限幅最小值
#define CtrlValuePsi_Max 22.57 // 偏航控制量限幅最大值
#define Offset_Min 0.05 // 侧偏控制有效最小值（m）
#define Psi_Min 0.5 // 偏航控制有效最小值（°）
#define SpeedGei 3 // 自动控制车速给定（km/h）
#define Wheel_Tread 3090 // 轮距（mm）
#define Wheel_Base 4587 // 轴距（mm）
#define PI 3.14159 // π


//输出控制量结构体
typedef struct
{
double LeftFrontWheel_Angle; // 左前轮角度——左偏为负
double RightFrontWheel_Angle; // 右前轮角度——左偏为负
double LeftBehindWheel_Angle; // 左后轮角度——左偏为负
double RightBehindWheel_Angle; // 右后轮角度——左偏为负
double Speed; // 车速——向后为负
}CtrlValue; //输出控制量


//变量
double CtrlValueOffset_P; //侧偏比例控制量（°）
double CtrlValueOffset_I; //侧偏积分控制量（°）
double CtrlValuePsi_P; //偏航比例控制量（°）
double CtrlValuePsi_I; //偏航积分控制量（°）
double CtrlValueOffset; //侧偏控制量（°）
double CtrlValuePsi; //偏航控制量（°）




/*********************************************************************************************************
*Funtion Limit
*Descreption:限幅函数,将数据限制在Min和Max之间
*Calls:
*Called by:
*Input:
*Output:
*Return:
*Others:
*********************************************************************************************************/
double Limit(double DataIn, double Min, double Max)
{
static double Data = 0;
if (DataIn<Min)
{
Data = Min;
}
else if (DataIn>Max)
{
Data = Max;
}
else
{
Data = DataIn;
}
return Data;
}




/*********************************************************************************************************
*Funtion LToR
*Descreption:计算函数,根据左前轮偏航控制量计算右前轮偏航控制量
*Calls:
*Called by:
*Input:
*Output:
*Return:
*Others:
*********************************************************************************************************/
double LToR(double LValue)
{
static double RValue = 0.0;
if(0.0 == LValue)
{
RValue = 0.0;
}
else
{
RValue = math.atan(1.0 / (1.0 / math.tan(LValue / 180.0 * PI) - Wheel_Tread / Wheel_Base));
}
return RValue;
}




/*********************************************************************************************************
*Funtion Ctrl
*Descreption:控制函数
*Calls:
*Called by:
*Input:Offset侧偏——当前位置在目标路线左边为左偏→负值；Psi偏航——当前朝向在目标点逆时针方向为左偏→负值
*Output:
*Return:
*Others:
*********************************************************************************************************/
CtrlValue Controler(double Offset, double Psi, int Phrase, double DistanceStop)
{
static CtrlValue DataOut;


/********************侧偏控制有效判断********************/
if (Offset_Min > abs(Offset))
{
Offset = 0;
}
/********************偏航控制有效判断********************/
if (Psi_Min >abs( Psi))
{
Psi = 0;
}


if (0 == Phrase)//急停状态，控制中断
{
DataOut.Speed = 0;
}
else//自动控制
{
if (abs(DistanceStop)<StopLimit)//控制距离是否足够判断
{
DataOut.Speed = 0;
}
else
{
DataOut.Speed = SpeedGei/3.6;//车速给定（m/s）

/*******************************侧偏控制器*******************************/
CtrlValueOffset_P = Offset * KpOffset;//侧偏比例控制
CtrlValueOffset_I += (Offset * KiOffset);//侧偏积分控制
CtrlValueOffset_I = Limit(CtrlValueOffset_I, CtrlValueOffset_IMin, CtrlValueOffset_IMax);//侧偏控制量积分限幅
CtrlValueOffset = CtrlValueOffset_P + CtrlValueOffset_I;//侧偏控制量
CtrlValueOffset = Limit(CtrlValueOffset, CtrlValueOffset_Min, CtrlValueOffset_Max);//侧偏控制量限幅


/*******************************偏航控制器*******************************/
CtrlValuePsi_P = Psi * KpPsi;//偏航比例控制
CtrlValuePsi_I += (Psi * KiPsi);//偏航积分控制
CtrlValuePsi_I = Limit(CtrlValuePsi_I, CtrlValuePsi_IMin, CtrlValuePsi_IMin);//偏航控制量积分限幅
CtrlValuePsi = CtrlValuePsi_P + CtrlValuePsi_I;//偏航控制量
CtrlValuePsi = Limit(CtrlValuePsi, CtrlValuePsi_Min, CtrlValuePsi_Min);//偏航控制量限幅


  /******************************车轮角度输出******************************/
DataOut.LeftFrontWheel_Angle = CtrlValueOffset + CtrlValuePsi;//左前轮
DataOut.RightFrontWheel_Angle = CtrlValueOffset + LToR(CtrlValuePsi);//右前轮
DataOut.LeftBehindWheel_Angle = CtrlValueOffset;//左后轮
DataOut.RightBehindWheel_Angle = CtrlValueOffset;//右后轮
}
}
return DataOut;
}