智能车舵机PD运算

/**********************舵机增量式PID算法*********************** double ref = 0;//设置参数设定值

double feb = 0;//采样反馈过程值

int pwm_var = 0; //PID调整量

int PWM_out = 0; //PWM输出量

double Uo = 0;

double Ek = 0;

double Ei = 0;

double Ed = 0;

#define Kp 8 //PID调节的比例常数

#define Ti 0.05 //PID调节的积分常数

#define Td 0.02 //PID调节的微分时间常数

#define T 0.02 //采样周期

#define Kpp Kp * ( 1 + (T / Ti) + (Td / T) ) #define Ki (-Kp) * ( 1 + (2 * Td / T ) )

#define Kd Kp * Td / T

//#define Kpp 4

//#define Ki 0.8

//#define Kd 20

//误差的阀值，小于这个数值的时候，不做PID调整，避免误差较小时频繁调节引起震荡

#define Emin 3

//调整值限幅，防止积分饱和

#define Umax 100

#define Umin -100

//输出值限幅

#define Pmax 15500

#define Pmin 200

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

////// PID运算 ///////

void pid_ctrl(void)

{

Ek = ref - feb; //差值运算

if( fabs(Ek) < Emin ) //误差的阀值(死区控制??)

{

pwm_var = 0;

}

else

{

Uo = Kpp*Ek + Ki*Ei + Kd*Ed;//PID计算

Ed = Ei;

Ei = Ek;

pwm_var = (int)Uo; //制转化调整量,PWM为整数

if(pwm_var >= Umax)pwm_var = Umax; //调整值限幅，防止积分饱和 if(pwm_var <= Umin)pwm_var = Umin; //调整值限幅，防止积分饱和 }

PWM_out += pwm_var; //调整PWM输出

if(PWM_out > Pmax)PWM_out = Pmax; //输出值限幅

if(PWM_out < Pmin)PWM_out = Pmin; //输出值限幅

TBCCR1 = PWM_out;//输出给寄存器,改变PWM占空比

}

#define G_Kp 8 //P 8

#define G_Ki 15 //I

#define G_Kd 0 //D

int error;

int pre_error;

int last_error;

int U_error;

uint U_Pre1=9000; //摆头舵机中心位置

uint U_Pre2=8600; //方向舵机中心位置

uchar sum;

uchar temp,tt,ss;

/**************************************************

函数：delay_ms()

描述：

延迟x*10微秒

**************************************************/

void delay(uint time)

{

while(time--)

{

_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);

}

}

void display()

{

sum = 0;

error = 0;

/*********************************/

PORTA = 0x00;

delay(100);

if(PORTA_PA0 == 1)

{

sum++;

error -= 12;

}

/*********************************/ PORTA = 0x02;

delay(100);

if(PORTA_PA0 == 1)

{

sum++;

error -= 10;

}

/*********************************/ PORTA = 0x04;

delay(100);

if(PORTA_PA0 == 1)

{

sum++;

error -= 8;

}

/*********************************/ PORTA = 0x08;

delay(100);

if(PORTA_PA0 == 1)

{

sum++;

error -= 6;

}

/*********************************/ PORTA = 0x0a;

delay(100);

if(PORTA_PA0 == 1)

{

sum++;

error -= 4;

}

/*********************************/ PORTA = 0x0c;

delay(100);

if(PORTA_PA0 == 1)

{

sum++;

error -= 2;

}

/*********************************/ PORTA = 0x10;

delay(100);

if(PORTA_PA0 == 1)

{

sum++;

error += 2;

}

/*********************************/ PORTA = 0x12;

delay(100);

if(PORTA_PA0 == 1)

{

sum++;

error += 4;

}

/*********************************/ PORTA = 0x14;

delay(100);

if(PORTA_PA0 == 1)

{

sum++;

error += 6;

}

/*********************************/ PORTA = 0x18;

delay(100);

if(PORTA_PA0 == 1)

{

sum++;

error += 8;

}

/*********************************/ PORTA = 0x1a;

delay(100);

if(PORTA_PA0 == 1)

{

sum++;

error += 10;

}

/*********************************/ PORTA = 0x1c;

delay(100);

if(PORTA_PA0 == 1)

{

sum++;

error += 12;

}

PORTA = 0x17; //激光消影

/*********************************/

if((sum>0)&&(sum<5))

{

error = (int)(error/(sum));

if(error>4)

U_error = 220;

else if(error<-4)

U_error = -220;

else if(error>6)

U_error = 320;

else if(error<-6)

U_error = -320;

else if(error>8)

U_error = 420;

else if(error<-8)

U_error = -420;

else

U_error = ((G_Kp*(error-pre_error)+G_Ki*error+G_Kd*(error-2*pre_err or+last_error)));

if(U_error >420)

U_error = 420;

else if(U_error <-420)

U_error = -420;

U_Pre1 += U_error;

if(U_Pre1>12000)

U_Pre1 = 12000;

else if(U_Pre1<6000)

U_Pre1 = 6000;

PWMDTY01 = U_Pre1;

if(U_Pre1 > 9000) U_Pre2 = (8600+(((U_Pre1-9000)*9)/10));

else U_Pre2 = (8600-(((9000-U_Pre1)*7)/10));

if(U_Pre2 > 9800) U_Pre2 = 9800;

else if(U_Pre2 < 7400) U_Pre2 = 7400;

PWMDTY45 = U_Pre2;

}

last_error=pre_error;

pre_error=error;

/**********************/

//速度调节

if((U_Pre2>9600)||(U_Pre2<7600))

{

if(temp == 6)

{

PTP_PTP7 = 1; //电机正转

delay(100);

PTP_PTP7 = 0;

}

if(temp == 1){tt++;if(tt == 1)PWMDTY3 = 50;} else{tt = 0;temp = 1;}

}

else if((U_Pre2>9400)||(U_Pre2<7800))

{

if(temp == 2){tt++;if(tt == 2)PWMDTY3 = 70;} else{tt = 0;temp = 2;}

}

else if((U_Pre2>9200)||(U_Pre2<8000))

{

if(temp == 3){tt++;if(tt == 2)PWMDTY3 = 75;} else{tt = 0;temp = 3;}

}

else if((U_Pre2>9000)||(U_Pre2<8200))

{

if(temp == 4){tt++;if(tt == 2)PWMDTY3 = 80;} else{tt = 0;temp = 4;}

}

else if((U_Pre2>8700)||(U_Pre2<8500))

{

if(temp == 5){tt++;if(tt == 2)PWMDTY3 = 83;}

else{tt = 0;temp = 5; }

}

else if((U_Pre2<8700)||(U_Pre2>8500)) //直道

{

if(temp == 6){tt++;if(tt == 2)PWMDTY3 = 85;}

else{tt = 0;temp = 6; }

}

/************************************************************/ }