PID算法.docx - 冰豆网

资源描述

PID算法.docx

《PID算法.docx》由会员分享，可在线阅读，更多相关《PID算法.docx（53页珍藏版）》请在冰豆网上搜索。

PID算法.docx

PID算法

电机转速的调节

电机转速设置在软件上都通过pwm波占空比设置完成的。

voidPWM45_Init（void）

{PWME_PWME5=0;

PWMCNT45=0;

PWMDTY45=0;/*初始化程序*/

PWMPER45=24000;/*设定初值*/

PWMCTL_CON45=1;

PWMPRCLK=0;/*设置分频器的初值*/

PWMSCLA=1;/*Setscaleregister*/

PWMCLK_PCLK5=1;/*选择时钟来源*/

PWMPOL_PPOL5=1;

PWME_PWME5=1;/*开始*/

}

第六章舵机驱动模块

S12脉宽调制模块有8路独立的可设置周期和占空比的PWM通道，每个通道配有专门的计数器。

该模块有4个时钟源，它们为PWM波的设置提供了宽阔的频率范围。

通过该模块内寄存器可设置PWM的使能与否，每个通道的工作脉冲极性，每个通道输出的中间还是左对齐方式，时钟源，使用方式（是作为八个8位精度通道还是四个16位精度通道）。

该模块还有紧急关闭功能。

图6.2舵机电路

6.2程序代码

运行电机的转速以及舵机的转角，在软件上都是通过对PWM波占空比进行设置来相应控制的。

程序代码如下：

voidPWM67_Init（void）

{PWME_PWME7=0;

PWMCTL_CON67=1;

PWMPRCLK=0;

PWMSCLB=2;

PWMCLK_PCLK7=1;

PWMPOL_PPOL7=1;

PWMCNT67=0;

PWMDTY67=8875;

PWMPER67=48000;

PWME_PWME7=1;}

速度传感器

我们采用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））;

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）

{

pp->vl_PreU=VV_MAX;

}

elseif（pp->vl_PreU<=VV_MIN）

{

pp->vl_PreU=0;

}

if（error>20）{

pp->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/*commondefinesandmacros*/

#include/*derivativeinformation*/

#pragmaLINK_INFODERIVATIVE"mc9s12dg128b"

#include"7279.h"

#include"Rudder_Control.h"

#include"Motor_Control.h"

#include"TripMemory.h"

#include"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（）{

uchari;

StraightFlag=1;

for（i=0;i<19;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）

OutFlag=1;

break;

case3:

MSen_Data=3;

if（OutFlag==2）

OutFlag=1;

break;

case2:

MSen_Data=4;

break;

case6:

MSen_Data=5;

break;

case4:

MSen_Data=6;

break;

case12:

MSen_Data=7;

break;

case8:

MSen_Data=8;

break;

case24:

MSen_Data=9;

break;

case16:

MSen_Data=10;

break;

case48:

MSen_Data=11;

break;

case32:

MSen_Data=12;

break;

case96:

MSen_Data=13;

break;

case64:

MSen_Data=14;

break;

case192:

MSen_Data=15;

break;

case128:

MSen_Data=16;

break;

case384:

MSen_Data=17;

break;

case256:

MSen_Data=18;

break;

case768:

MSen_Data=19;

if（OutFlag==3）

OutFlag=1;

break;

case512:

MSen_Data=20;

if（OutFlag==3）

OutFlag=1;

break;

case896:

MSen_Data=22;

break;

case7:

MSen_Data=23;

break;

case448:

MSen_Data=24;

break;

case14:

MSen_Data=25;

break;

case224:

MSen_Data=26;

break;

case112:

MSen_Data=27;

break;

case56:

MSen_Data=28;

break;

case28:

MSen_Data=29;

break;

////////////////////////////////////////////

//case7: