导航车体驱动代码

原创 2018年04月16日 11:21:34
#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;
}

磁导航组成与基本原理

1.系统组成及工作流程 1.1主要组成部分 1)上位机系统       管理、监控小车的运行状态和相关参数,实时做出调度决策。 2)网络数据传输       数据传输、共享的硬件保障,...
  • wangguchao
  • wangguchao
  • 2017-11-28 19:23:55
  • 568

导航用到的各种坐标系的定义

以下为网络转载: GPS是全球性的定位导航系统的坐标系统是全球性的,它是通过国际协议确定的协议地球坐标系(Coventional Terrestial System—CTS)。目前,GPS测量中所使...
  • jyyh_ycy
  • jyyh_ycy
  • 2017-08-26 17:32:54
  • 464

导航用蓝牙驱动

  • 2014年07月30日 16:28
  • 1.44MB
  • 下载

VFD汽车显示与车体控制系统

  • 2011年05月14日 08:15
  • 1.88MB
  • 下载

导航代码导航代码导航代码导航代码导航代码导航代码导航代码导航代码导航代码导航代码

  • 2009年06月09日 17:10
  • 200KB
  • 下载

制作一个MSN机器人ing1

想制作一个MSN机器人,看了MSN 协议介绍和MSN的网络结构介绍,发现还是挺烦琐的,想找一个开源的来用一用,还找到了一个TjMSN,下载了它的程序包 通过 运行 java -cp TjMSN.jar...
  • floraman
  • floraman
  • 2008-06-14 11:19:00
  • 213

智能小车 第2版

  • 2013年08月06日 16:29
  • 3.8MB
  • 下载

车体广告机动车车体的变更

  • 2008年09月05日 16:09
  • 26KB
  • 下载

WinCE 5.0下鼠标键盘驱动分析

  本文通过对WinCE 5.0下的鼠标键盘驱动分析,对WinCE驱动程序设计进行了分析。欢迎大家对不对的地方指出。         硬件       写一个驱动程序的第一件事就是读硬件的规范文档。所...
  • dding832
  • dding832
  • 2008-01-12 14:35:00
  • 1187

用AT89C2051制作的寻迹机器人

2006-05-10 22:48:39 用AT89C2051制作的寻迹机器人 用AT89C2051制作...
  • mybirdsky
  • mybirdsky
  • 2008-01-17 16:06:00
  • 321
收藏助手
不良信息举报
您举报文章:导航车体驱动代码
举报原因:
原因补充:

(最多只允许输入30个字)