这样一个程序

#include "control.h"

#define T 0.156f

#define L 0.1445f

#define K 311.4f

u8 Flag_Target;

int Voltage_Temp,Voltage_Count,Voltage_All,sum;

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

函数功能：小车运动数学模型

入口参数：速度和转角

返回  值：无

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

void Kinematic_Analysis(float velocity,float angle)

{

Target_A=velocity*(1+T*tan(angle)/2/L);

Target_B=velocity*(1-T*tan(angle)/2/L);      //后轮差速

Servo=SERVO_INIT+angle*K;                    //舵机转向

}

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

函数功能：所有的控制代码都在这里面

5ms定时中断由MPU6050的INT引脚触发

严格保证采样和数据处理的时间同步

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

void TIM2_IRQHandler(void)

{

if(TIM2->SR&0X0001)//定时中断

{

TIM2->SR&=~(1<<0);                                                                                               //===清除定时器中断标志位

Flag_Target=!Flag_Target;

if(delay_flag==1)

{

if(++delay_50==10)         delay_50=0,delay_flag=0;                      //给主函数提供50ms的精准延时

}

if(Flag_Target==1)

{

Key();                                       //===扫描按键状态 单击双击可以改变小车运行状态

if(Flag_Way==2)

{

RD_TSL();                                            //===读取线性CCD数据

Find_CCD_Zhongzhi();                                                   //===提取中线

}

if(Flag_Way==3)

{

Sensor_Left=Get_Adc(3);                //采集左边电感的数据

Sensor_Right=Get_Adc(8);               //采集右边电感的数据

Sensor_Middle=Get_Adc(2);              //采集中间电感的数据

sum=Sensor_Left*1+Sensor_Middle*100+Sensor_Right*199;  //归一化处理

Sensor=sum/(Sensor_Left+Sensor_Middle+Sensor_Right);   //求偏差

}

}

if(Flag_Target==0)

{

Encoder_Left=-Read_Encoder(2);                                       //===读取编码器的值                                                         //为了保证M法测速的时间基准，首先读取编码器数据

Encoder_Right=-Read_Encoder(3);                                      //===读取编码器的值

Get_RC();

Kinematic_Analysis(Velocity,Angle);                                                                                                                             //小车运动学分析

if(Turn_Off(Voltage)==0)                                                                                       //===如果不存在异常

{

Motor_A=Incremental_PI_A(Encoder_Left,Target_A);                   //===速度闭环控制计算电机A最终PWM

Motor_B=Incremental_PI_B(Encoder_Right,Target_B);                  //===速度闭环控制计算电机B最终PWM

Xianfu_Pwm();                                                      //===PWM限幅

Set_Pwm(Motor_A,Motor_B,Servo);                                          //===赋值给PWM寄存器

}

else

Set_Pwm(0,0,SERVO_INIT);                                                                                  //===赋值给PWM寄存器

Voltage_Temp=Get_battery_volt();                                                 //=====读取电池电压

Voltage_Count++;                                                     //=====平均值计数器

Voltage_All+=Voltage_Temp;                                           //=====多次采样累积

if(Voltage_Count==100) Voltage=Voltage_All/100,Voltage_All=0,Voltage_Count=0;//=====求平均值

}

if(Flag_Show==0)                                Led_Flash(100);

else if(Flag_Show==1)        Led_Flash(0);

}

}

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

函数功能：赋值给PWM寄存器

入口参数：左轮PWM、右轮PWM

返回  值：无

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

void Set_Pwm(int motor_a,int motor_b,int servo)

{

if(Flag_Way>=2)//巡线模式下，只允许电机正转

{

if(motor_a<0)motor_a=0;

if(motor_b<0)motor_b=0;

}

if(motor_a<0)                        PWMA1=7200,PWMA2=7200+motor_a;

else                     PWMA2=7200,PWMA1=7200-motor_a;

if(motor_b<0)                        PWMB1=7200,PWMB2=7200+motor_b;

else                     PWMB2=7200,PWMB1=7200-motor_b;

SERVO=servo;

}

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

函数功能：限制PWM赋值

入口参数：无

返回  值：无

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

void Xianfu_Pwm(void)

{

int Amplitude=6900;    //===PWM满幅是7200 限制在6900

if(Motor_A

if(Motor_A>Amplitude)  Motor_A=Amplitude;

if(Motor_B

if(Motor_B>Amplitude)  Motor_B=Amplitude;

if(Servo

if(Servo>(SERVO_INIT+250))    Servo=SERVO_INIT+250;                  //舵机限幅

}

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

函数功能：按键修改小车运行状态

入口参数：无

返回  值：无

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

void Key(void)

{

u8 tmp,tmp2;

tmp=click();

if(tmp==1)Flag_Stop=!Flag_Stop;//单击控制小车的启停

//if(tmp==2)Flag_Show=!Flag_Show;//双击控制小车的显示状态

tmp2=Long_Press();

if(tmp2==1)Flag_Show=!Flag_Show;//控制小车的显示状态

}

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

函数功能：异常关闭电机

入口参数：电压

返回  值：1：异常  0：正常

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

u8 Turn_Off( int voltage)

{

u8 temp;

if(voltage<740||Flag_Stop==1)//电池电压低于7.4V关闭电机

{

temp=1;

}

else

temp=0;

return temp;

}

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

函数功能：绝对值函数

入口参数：int

返回  值：unsigned int

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

int myabs(int a)

{

int temp;

if(a<0)  temp=-a;

else temp=a;

return temp;

}

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

函数功能：增量PI控制器

入口参数：编码器测量值，目标速度

返回  值：电机PWM

根据增量式离散PID公式

pwm+=Kp[e(k)-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]

e(k)代表本次偏差

e(k-1)代表上一次的偏差  以此类推

pwm代表增量输出

在我们的速度控制闭环系统里面，只使用PI控制

pwm+=Kp[e(k)-e(k-1)]+Ki*e(k)

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

int Incremental_PI_A (int Encoder,int Target)

{

static int Bias,Pwm,Last_bias;

Bias=Target-Encoder;                //计算偏差

Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;   //增量式PI控制器

Last_bias=Bias;                           //保存上一次偏差

return Pwm;                         //增量输出

}

int Incremental_PI_B (int Encoder,int Target)

{

static int Bias,Pwm,Last_bias;

Bias=Target-Encoder;                //计算偏差

Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;   //增量式PI控制器

Last_bias=Bias;                           //保存上一次偏差

return Pwm;                         //增量输出

}

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

函数功能：线性CCD取中值

入口参数：无

返回  值：无

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

void  Find_CCD_Zhongzhi(void)

{

static u16 i,j,Left,Right,Last_CCD_Zhongzhi;

static u16 value1_max,value1_min;

value1_max=ADV[0];  //动态阈值算法，读取最大和最小值

for(i=5;i<123;i++)   //两边各去掉5个点

{

if(value1_max<=ADV[i])

value1_max=ADV[i];

}

value1_min=ADV[0];  //最小值

for(i=5;i<123;i++)

{

if(value1_min>=ADV[i])

value1_min=ADV[i];

}

CCD_Yuzhi=(value1_max+value1_min)/2;          //计算出本次中线提取的阈值

for(i = 5;i<118; i++)   //寻找左边跳变沿

{

if(ADV[i]>CCD_Yuzhi&&ADV[i+1]>CCD_Yuzhi&&ADV[i+2]>CCD_Yuzhi&&ADV[i+3]

{

Left=i;

break;

}

}

for(j = 118;j>5; j--)//寻找右边跳变沿

{

if(ADV[j]CCD_Yuzhi&&ADV[j+4]>CCD_Yuzhi&&ADV[j+5]>CCD_Yuzhi)

{

Right=j;

break;

}

}

CCD_Zhongzhi=(Right+Left)/2;//计算中线位置

//        if(myabs(CCD_Zhongzhi-Last_CCD_Zhongzhi)>90)   //计算中线的偏差，如果太大

//        CCD_Zhongzhi=Last_CCD_Zhongzhi;    //则取上一次的值

//        Last_CCD_Zhongzhi=CCD_Zhongzhi;  //保存上一次的偏差

}