创新设计—自动寻迹小车(7)

Turn=18; mid_flag=0; right_close=1; }

if(AD== 1921) {

Turn=19;mid_flag=0; right_close=1; }

if(AD== 21) {

Turn=20; mid_flag=0; right_close=1; }

if(AD== 2321) {

Turn=21; mid_flag=0; right_close=1; }

if(AD== 2300) {

Turn=22;mid_flag=0; right_close=1; } } }

/*************************PID初始化**********************/ void PIDInit(void) {

sPID.ThisPoint=0; //设定目标 Desired value

sPID.Kp=0; //比例常数 Proportional Const sPID.Ki=0; //积分常数 Integral Const sPID.Kd=0; //微分常数 Derivative Const sPID.LastError=0; //Error[-1] sPID.PrevError=0; //Error[-2]

sPID.SumError=0; //Sums of Errors

DjPID.ThisPoint=0; //设定目标 Desired value 302

DjPID.Kp=1; //比例常数 Proportional Const 50 DjPID.Ki=1; //积分常数 Integral Const DjPID.Kd=3; //微分常数 Derivative Const DjPID.LastError=0; //Error[-1] DjPID.PrevError=0; //Error[-2]

DjPID.SumError=0; //Sums of Errors }

/*************************IO初始化**********************/ void IOInit(void) {

PTGDD_PTGDD4=0; //G4复位按键 PTGPE_PTGPE4=1;

PTGDD_PTGDD3=1; /*G3连接计数芯片4040清零管脚，高电平清零*/

31

PTGD_PTGD3=0;

PTGDD_PTGDD1=1; /*G1拉高蜂鸣器，高电平有效，强驱动*/ PTGDS_PTGDS1=1; PTGD_PTGD1=1; PTGD_PTGD1=0;

PTEDD=0x00; /*E 测速计数输入*/

PTCDD_PTCDD6=1; /*C6连接驱动芯片386的IN1*/ PTFDD_PTFDD7=1; /* F7连接驱动芯片386的IN2*/ PTCD_PTCD6=0; PTFD_PTFD7=1; }

/*************************时钟初始化**********************/ void ICGInit(void) {

ICGC1=0x78; /*fll系数为p=1,外部晶振*/ ICGC2=0x20; /*倍频因子N=8，分频因子R=1*/ }

/*************************电机舵机初始化**********************/ void servoInit(void) {

TPM2SC=0x0F; /*总线时钟128分频*/ TPM2MOD=2500;

TPM2C1SC=0x28; /*舵机初始化------通道0设置边缘对齐的PWM，中断请求不允许*/

TPM2C1V=185;

TPM2C0SC=0x28; /*电机初始化------通道0设置边缘对齐的PWM，中断请求不允许*/

TPM2C0V=0; /*设定初始占空比，舵机处于中间位置*/ }

/*************************延时****************************/ void delay(int de){

while(de--){ __RESET_WATCHDOG();} }

void delay1(int de) {

int i=100; while(de--) while(i--)

__RESET_WATCHDOG(); }

void delay2(int count) /* 延时子程序*/ {

int i,j,k;

32

for(i=0;i

for(j=0;j<100;j++) {

for(k=0;k<800;k++) {

__RESET_WATCHDOG(); /*复位看门狗计数器*/ } } } }

/*************************溢出中断处理*******************/ interrupt VectorNumber_Vtpm3ovf void TPM3OVF_ISR(void) {

DisableInterrupts;

if((TPM3SC & 0x80)==0x80) {

TPM3SC_TOF = 0; }

speed=PTED&0x3F;;

PTGD_PTGD3=1;//计数器清零 PTGD_PTGD3=0;

if((Turn>7)&&(Turn<15)) {

l[0]=1; } else {

l[0]=0;

if(Turn>14) r=1; if(Turn<8) r=2; }

zw[10]=zw[9]; zw[9]=zw[8]; zw[8]=zw[7]; zw[7]=zw[6]; zw[6]=zw[5]; zw[5]=zw[4]; zw[4]=zw[3]; zw[3]=zw[2]; zw[2]=zw[1]; zw[1]=zw[0]; zw[0]=l[0]; EnableInterrupts;

33

}

/*********************速度PID****************************/ int speedPIDCalc (speedPID *pp) {

int Error,dError;

Error=pp->ThisPoint-pp->NextPoint; //偏差e(k) pp->SumError+=Error; //积分 e(1)+e(2)+******* dError=Error-pp->LastError; //当前微分 pp->PrevError=pp->LastError; pp->LastError=Error;

if((w>=1500)||((Turn>14)||(Turn<8))) {

sPID.Kp=80; // 110 sPID.Ki=0; // 0 sPID.Kd=10; // 10 } else

{ // 145 sPID.Kp=180; // 1 sPID.Ki=0; // 15 sPID.Kd=5; }

return (pp->Kp*Error+pp->Ki*pp->SumError/50+pp->Kd*dError); }

/*********************舵机PID****************************/ int steerPIDCalc (steerPID *pp) {

int dError,Error;

Error=pp->ThisPoint-pp->NextPoint; //偏差 pp->SumError+=Error; //积分

dError=pp->LastError-pp->PrevError; //当前微分 pp->PrevError=pp->LastError; pp->LastError=Error;

return (pp->Kp*Error+pp->Ki*pp->SumError/2500+pp->Kd*dError); }

/*********************舵机控制****************************/ void SteerContral(void) {

int djpid;

DjPID.NextPoint=0;

DjPID.ThisPoint=Zhuanjiao[Turn]; //turn为黑线位置 if(v>0) {

djpid=v;

34

} else {

djpid=101+steerPIDCalc(&DjPID); } v=0;

TPM2C1V=djpid; a=TPM2C1V; }

/*********************电机控制****************************/ void SpeedContral(void) {

if(circle>=2) {

TPM2C0V=2500; PTCD_PTCD6=0; PTFD_PTFD7=0; for(;;) {

__RESET_WATCHDOG(); } } else {

sPID.NextPoint=speed;

sPID.ThisPoint=Shuruspeed[Turn]; if(u>0) {

uk=u; } else {

uk=speedPIDCalc(&sPID); } u=0;

TPM2C0V=uk; w=TPM2C0V; } }

/*********************起始延时***************************/ void Start_Key(void) {

for(;;) {

35