/**********************舵机增量式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 //Dint 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);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_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)else if(error>8)U_error = 420;else if(error<-8)U_error = -420;elseU_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);}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; }}/************************************************************/ }。