PID算法Word下载.docx
《PID算法Word下载.docx》由会员分享，可在线阅读，更多相关《PID算法Word下载.docx（53页珍藏版）》请在冰豆网上搜索。
}
速度传感器
我们采用PT0口作为脉冲信号输入。
采用模数向下计数器，首先通过设置寄存器MCCTL，设置模数方式使能，向下中断使能，向下计数使能。
设置FLMC位，使得载入寄存器进入模数计数方式。
设置MCCNT寄存器，使得定时器每20ms中断一次。
然后设置TIOS，设置PT0针脚为输入；
然后设置TCTL4寄存器，选择获取上升沿。
之后，设置PBCTL寄存器，将PAC0和PAC1合用。
设置代码为：
MCFLG_MCZF=1;
MCCTL_MODMC=1;
MCCTL_MCZI=1;
MCCTL_MCEN=1;
MCCNT=30000;
MCCTL_MCPR=3;
MCCTL_FLMC=1;
TIOS_IOS0=0;
TCTL4=0x01;
PBCTL_PBEN=1;
在中断服务函数中，通过读取PACN01这个寄存器，获取当前的脉冲累加值。
例程如下
voidInt_mcount（void）
{MCFLG_MCZF=1;
pulse=PACN10;
//读出脉冲计数值
PACN10=0;
sPID.vi_FeedBack=pulse;
PWMDTY45=（unsignedint）v_PIDCalc（&
sPID）;
}
在每一控制周期开始，S12读取脉冲累加器中的数值，然后将脉冲累加器清零。
这样就求得了之前m个控制周期时间里共有多少个脉冲数累积，从而由公式3-1可求得赛车速度值。
程序流程图7.2：
图7.2速度传感模块程序流程图
7．4速度传感器准确性测试
传统的调试方法是调车者跟据智能车绕行赛道一圈的时间和肉眼观测到的智能车在赛道上的行驶路径作为修改智能车参数的指标，然后反复测试，反复修改，以达到最好的效果。
这样的调试比较盲目，缺乏目的性，也浪费了大量的时间和精力在做重复性劳动，效果不好。
并且，由于光电传感器本身的特性，使得前瞻性远不如ccd图象识别，使得电机控制效果很不好；
并且，由于采集信号是离散的，转角的控制也显得不平滑。
这时，为了能和ccd图像识别进行抗衡，赛道记忆策略就发挥了作用。
赛道记忆算法是指在比赛的第一圈以最安全的速度缓慢驶过一圈,并将赛道信息保存下来，第二圈根据保存下来的信息进行车速和转角决策的相应最优化，从而在第二圈取得好成绩，这一算法在目前智能车竞赛中是比较先进的。
要实现赛道记忆算法，智能车运动状态实时监测系统是必不可少的。
8．1智能车运动状态实时监测系统总述
本系统主要完成将智能车行驶过程中的各种状态信息（如传感器亮灭，车速，舵机转角，电池电量等）实时地以无线串行通信方式发送至上位机处理，并绘制各部分状态值关于时间的曲线。
有了这些曲线就不难看出智能车在赛道各个位置的状态，各种控制参数的优劣便一目了然了。
尤为重要的是对于电机控制PID参数的选取，通过速度—时间曲线可以很容易发现各套PID参数之间的差异。
对于采用CCD传感器的队伍来说，该系统便成为了调试者的眼睛，可以见智能车之所见，相信对编写循线算法有很大帮助。
而且还可以对这些数据作进一步处理，例如求取一阶导数，以得到更多的信息。
8．1．1系统整体硬件架构
设计方案主要分成四部分：
起跑信号发送装置，车载数据采集系统，手持监测系统，PC机数据处理系统。
系统基本构建如下图2-1所示。
图8.1系统基本架构示意图
V_PID.H
#defineVV_KPVALUE10
#defineVV_KIVALUE4
#defineVV_KDVALUE8
#defineVV_MAX1200
#defineVV_MIN0
#defineVV_DEADLINE0x00
typedefstructPID
{
signedintvi_Ref;
signedintvi_FeedBack;
signedlongvi_PreError;
signedlongvi_PreDerror;
unsignedintv_Kp;
unsignedintv_Ki;
unsignedintv_Kd;
signedlongvl_PreU;
unsignedlongvl_Pre;
}PID;
PIDsPID;
//PIDControlStructure
signedlongv_PIDCalc（PID*pp）
signedlongerror,d_error,dd_error;
error=（signedlong）（pp->
vi_Ref-pp->
vi_FeedBack）;
d_error=error-pp->
vi_PreError;
dd_error=d_error-pp->
vi_PreDerror;
pp->
vi_PreError=error;
pp->
vi_PreDerror=d_error;
if（（error<
VV_DEADLINE）&
&
（error>
-VV_DEADLINE））;
else
{
pp->
vl_PreU+=（signedlong）（pp->
v_Kp*d_error+pp->
v_Ki*error+pp->
v_Kd*dd_error）;
if（pp->
vl_PreU>
=VV_MAX）
vl_PreU=VV_MAX;
elseif（pp->
vl_PreU<
=VV_MIN）
vl_PreU=0;
}
if（error>
20）{
vl_PreU=VV_MAX;
return（pp->
vl_PreU）;
}
voidPIDInit（）
{
sPID.vi_Ref=0;
sPID.vi_FeedBack=0;
sPID.vi_PreError=0;
sPID.vi_PreDerror=0;
sPID.v_Kp=VV_KPVALUE;
sPID.v_Ki=VV_KIVALUE;
sPID.v_Kd=VV_KDVALUE;
sPID.vl_PreU=0;
main.c
#include<
hidef.h>
/*commondefinesandmacros*/
mc9s12dg128.h>
/*derivativeinformation*/
#pragmaLINK_INFODERIVATIVE"
mc9s12dg128b"
#include"
7279.h"
Rudder_Control.h"
Motor_Control.h"
TripMemory.h"
DataTransfer.h"
uintSen_Data;
uintModulateSensor_Data;
uintModulateSensor_Data_Pre;
ucharOutFlag;
ucharMSensorDataRecord[20];
ucharStraightFlag;
ucharFlash;
longStartPos;
ucharSen_Data5;
ucharModulateSensor_Data5;
ucharModulateSensor_Data5_Pre;
ucharOutFlag5;
ucharStartSenData;
ucharJudgementCollect;
voids12clk_init（）{
PLLCTL=0xFF;
SYNR=0x02;
REFDV=0x01;
while（CRGFLG_LOCK==0）{}
CLKSEL_PLLSEL=1;
voidECT1_Init（）{
TSCR2_PR=0;
//prescalefactoris1,busclock/2=24Mhz
TSCR2_TOI=0;
//timeroverflowinterruptdisable
TSCR1_TEN=1;
//timerenable
TIOS_IOS1=1;
//setIOS1tooutputcompare
TC1=1000;
//outputcomparevalueis2400
TIE_C1I=1;
//outputcompareinterrupt1enable
voiddevice_init（）{
ucharinit_databass;
DDRA=0xFC;
DDRB=0x00;
DDRT=0x62;
DDRS=0x03;
DDRM=0x3C;
ATD0DIEN=0xFF;
PTT_PTT6=0;
s12clk_init（）;
ECT1_Init（）;
init_7279（）;
PWM67_Init（）;
Motor_Init（）;
OutFlag=1;
StartIdentification=0;
CrossIdentification=0;
DisControl=20000;
CheckStateFlag=0;
FinalJudge=0;
FirstJ=0;
JudgementCollect=0;
QD_Dis=20000;
test=9;
test2=0;
test3=0;
test4=0;
for（init_databass=1;
init_databass<
98;
init_databass++）{
TrailInfoSave[init_databass].Location=0xFFFFFF;
TrailInfoSave[init_databass].TrailType=0xFF;
TrailInfoSave[98].Location=0;
TrailInfoSave[98].TrailType=1;
TrailInfoSave[99].Location=0;
TrailInfoSave[99].TrailType=1;
Memory_init（）;
StrFlag=0;
RushingLength=80000;
StrDistance=20000;
voidSensorData_Record（）{
uchari;
for（i=0;
i<
19;
i++）{
MSensorDataRecord[19-i]=MSensorDataRecord[18-i];
MSensorDataRecord[0]=ModulateSensor_Data;
voidStraightJudge（）{
StraightFlag=1;
i++）{
if（（MSensorDataRecord[i]<
8）||（MSensorDataRecord[i]>
14））
StraightFlag=2;
uintSensor_Exchange（）{uintMSen_Data;
MSen_Data=0;
SensorState=1;
switch（Sen_Data）{
case1:
MSen_Data=2;
if（OutFlag==2）
break;
case3:
MSen_Data=3;
case2:
MSen_Data=4;
case6:
MSen_Data=5;
case4:
MSen_Data=6;
case12:
MSen_Data=7;
case8:
MSen_Data=8;
case24:
MSen_Data=9;
case16:
MSen_Data=10;
case48:
MSen_Data=11;
case32:
MSen_Data=12;
case96:
MSen_Data=13;
case64:
MSen_Data=14;
case192:
MSen_Data=15;
case128:
MSen_Data=16;
case384:
MSen_Data=17;
case256:
MSen_Data=18;
case768:
MSen_Data=19;
if（OutFlag==3）
case512:
MSen_Data=20;
case896:
MSen_Data=22;
case7:
MSen_Data=23;
case448:
MSen_Data=24;
case14:
MSen_Data=25;
case224:
MSen_Data=26;
case112:
MSen_Data=27;
case56:
MSen_Data=28;
case28:
MSen_Data=29;
////////////////////////////////////////////
//case7:
ModulateSensor_Data=ModulateSensor_Data_Pre;
//case14:
//case28:
//case56:
//case112:
ModulateSensor_Data=ModulateSensor_Data_Pre;
//case224:
//case448:
//case896:
/////////////////////////////////////////
default:
MSen_Data=ModulateSensor_Data_Pre;
SensorState=0;
//传感器状态表明当前状态不稳定
returnMSen_Data;
ucharSensorBack_Exchange（）{ucharMBSen_Data;
MBSen_Data=0;
switch（Sen_Data5）{
//////////////////left///////////////////
MBSen_Data=2;
OutFlag5=1;
MBSen_Data=3;
MBSen_Data=4;
MBSen_Data=5;
MBSen_Data=6;
OutFlag5=2;
MBSen_Data=7;
MBSen_Data=8;
MBSen_Data=9;
MBSen_Data=10;
OutFlag5=3;
///////////////////////////////////////////////////////////////////
MBSen_Data=ModulateSensor_Data5_Pre;
returnMBSen_Data;
#pragmaCODE_SEGNON_BANKED
#pragmaTRAP_PROC
voidECT1（void）{
TFLG1_C1F=1;
//clearflag
switch（TC1）{
case1000:
TC1=7000;
Sen_Data=0;
Sen_Data5=0;
PORTA_BIT6=1;
PORTA_BIT7=0;
PTT_PTT1=0;
PORTA_BIT2=1;
PORTA_BIT3=0;
PORTA_BIT4=0;
break;
case7000:
TC1=31000;
Sen_Data=Sen_Data+（uint）（PORTAB&
0x210）;
Sen_Data5=Sen_Data5+（uchar）（PORTAD0&
0x80）;
PORTA_BIT6=0;
PORTA_BIT2=0;
Motor_Control（ModulateSensor_Data,JudgementCollect,&
StrFlag,&
SpeedUpLength,&
RushingLength）;
case31000:
TC1=34000;
PORTA_BIT7=1;
PORTA_BIT2=0;
PORTA_BIT3=1;
case34000:
TC1=35000;
0x108）;
0x40）;
break;
case35000:
TC1=38000;
case38000:
TC1=39000;
0x84）;
0x20）;
case39000:
TC1=42000;
PTT_PTT1=1;
PORTA_BIT4=1;
case42000:
TC1=44000;
0x42）;
0x
升级会员 