STM32控制编码器电机实现【速度闭环控制】与【位置闭环控制】

最新推荐文章于 2024-10-16 11:36:07 发布

Ehang_Maker

最新推荐文章于 2024-10-16 11:36:07 发布

阅读量3.4w

收藏 1.2k

点赞数 151

分类专栏： STM32开发实践文章标签： stm32 arm 编码器电机速度闭环控制位置闭环控制

本文链接：https://blog.csdn.net/qq_44343584/article/details/122318756

版权

STM32开发实践专栏收录该内容

10 篇文章 30 订阅

订阅专栏

一、硬件及接线说明

本实验所基于的硬件分别为：

STM32F103C8T6 主控板
TB6612FNG 直流电机驱动模块
6线正交编码器电机（带AB相）

其中硬件接线为：

PWMA —— PA8
AIN1 —— PB14
AIN2 —— PB15
STBY —— 5V
编码器A相 —— PA1
编码器B相 —— PA0

STM32定时器资源分配：

定时器1（TIM1）：产生PWM波，作为TB6612的输入，控制电机进行调速；
定时器2（TIM2）：读取编码器的波形；
定时器3（TIM3）：产生周期为10ms的定时器中断，为控制系统提供稳定的时间基准。

【说明】上述硬件平台和接线仅给读者提供参考，更换主控或接线方式，请自行对示例程序进行微调。本文对于编码器的工作原理不加赘述，对于其原理请读者自行查阅相关资料。

二、速度闭环控制程序逻辑

【说明】下述程序中 control.c 最为重要，包含了速度闭环控制器的详细代码。其他程序模块供读者初始化参考。

main.c （主函数）

u8 flag_Stop=1;     //停止标志位
int Encoder;        //编码器的脉冲计数
int moto;           //电机PWM变量
int main(void)
 {	
	 Stm32_Clock_Init(9);      //系统时钟设置
     delay_init();             //=====延时初始化
	 LED_Init();               //=====初始化与 LED 连接的硬件接口
	 uart_init(115200);        //=====初始化串口1
	 MOTO_Init();              //初始化控制电机所需的IO
	 pwm_Init(7199,0);         //初始化pwm输出
	 Encoder_Init_TIM2();      //初始化计数器（定时器）
	 TIM3_Int_Init(99,7199);   //10ms一次中断
	 while(1)
	 {
	   printf("Encoder:%d \r\n",Encoder);
	 }
 }

moto.c （电机初始化相关函数）

void MOTO_Init(void)//初始化控制电机所需的IO
{
  GPIO_InitTypeDef GPIO_InitStruct;
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
	//PORTB12 13 14 15推挽输出
	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15;
	GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOB,&GPIO_InitStruct);	
}

void pwm_Init(u16 arr,u16 psc) //初始化pwm输出引脚
{	
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
	TIM_OCInitTypeDef TIM_OCInitStruct;
    GPIO_InitTypeDef GPIO_InitStruct;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1,ENABLE);  //使能定时器1时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE); //使能GPIOA的时钟
	
	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF_PP;         //复用输出
	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_11;   //PA8 PA11
	GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA,&GPIO_InitStruct);
	
	TIM_TimeBaseInitStruct.TIM_Period = arr;                     //设定计数器自动重装值 
	TIM_TimeBaseInitStruct.TIM_Prescaler  = psc;                 //设定预分频器
	TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
	TIM_TimeBaseInitStruct.TIM_ClockDivision = 0;                //设置时钟分割
	TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);              //初始化定时器
	
	TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM2;               //选择PWM2模式
	TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;   //比较输出使能
	TIM_OCInitStruct.TIM_Pulse = 0;                              //设置待装入捕获比较寄存器的脉冲值
	TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High;       //设置输出极性
	TIM_OC1Init(TIM1,&TIM_OCInitStruct);                         //初始化输出比较参数
	
	TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable); //CH1使能预装载寄存器
	 
	TIM_CtrlPWMOutputs(TIM1,ENABLE);                 //高级定时器输出必须设置这句
	
	TIM_ARRPreloadConfig(TIM1, ENABLE);              //使能TIM1在ARR上的预装载寄存器
	
	TIM_Cmd(TIM1,ENABLE);                            //使能定时器1
}

encoder.c （编码器初始化函数）

#include "encoder.h"
/**************************************************************************
函数功能：把TIM2初始化为编码器接口模式
入口参数：无
返回  值：无
**************************************************************************/
void Encoder_Init_TIM2(void)
{
  TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;  
  TIM_ICInitTypeDef TIM_ICInitStructure;  
  GPIO_InitTypeDef GPIO_InitStructure;
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);//使能定时器2的时钟
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);//使能PA端口时钟
	
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;	//端口配置
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
  GPIO_Init(GPIOA, &GPIO_InitStructure);			    //根据设定参数初始化GPIOB
  
  TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
  TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 预分频器 
  TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD-1; //设定计数器自动重装值
  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频：不分频
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_CenterAligned1;TIM向上计数  
  TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
	
  TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式3
	
  TIM_ICStructInit(&TIM_ICInitStructure); //把TIM_ICInitStruct 中的每一个参数按缺省值填入
  TIM_ICInitStructure.TIM_ICFilter = 10;  //设置滤波器长度
  TIM_ICInit(TIM2, &TIM_ICInitStructure); //根据 TIM_ICInitStruct 的参数初始化外设	TIMx
 
  TIM_ClearFlag(TIM2, TIM_FLAG_Update);//清除TIM的更新标志位
  TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);//使能定时器中断

  TIM_SetCounter(TIM2,0);//设置TIMx 计数器寄存器值
  TIM_Cmd(TIM2, ENABLE); //使能定时器2
}

//中断处理函数为空，清除中断标志位后结束中断
void TIM2_IRQHandler(void)
{
 if(TIM_GetFlagStatus(TIM2,TIM_FLAG_Update)==SET)//溢出中断
 {
	 TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位 
 }
}

/**************************************************************************
函数功能：单位时间读取编码器计数
入口参数：定时器
返回  值：速度值
**************************************************************************/
int Read_Encoder(u8 TIMX)//读取计数器的值
{
  int Encoder_TIM;
	switch(TIMX)
	{
	  case 2:Encoder_TIM=(short)TIM2->CNT; TIM2 -> CNT=0;  break;
	  case 3:Encoder_TIM=(short)TIM3->CNT; TIM3 -> CNT=0;  break;
	  case 4:Encoder_TIM=(short)TIM4->CNT; TIM4 -> CNT=0;  break;
	  default: Encoder_TIM=0;
	}
  return Encoder_TIM;
}

timer.c （定时器中断初始化函数）

/**************************************************************************
函数功能：定时中断初始化
入口参数：arr：自动重装值  psc：时钟预分频数 
返回  值：无
**************************************************************************/
void TIM3_Int_Init(u16 arr,u16 psc)
{
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
	NVIC_InitTypeDef NVIC_InitStruct;
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);
	
	TIM_TimeBaseInitStruct.TIM_Period = arr;     //重装载值
	TIM_TimeBaseInitStruct.TIM_Prescaler = psc;  //预分频系数
	TIM_TimeBaseInitStruct.TIM_ClockDivision =0; //时钟分割
	TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
	TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitStruct);
	
	TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE);  //使能定时器中断
	
	NVIC_InitStruct.NVIC_IRQChannel = TIM3_IRQn;   //使能外部中断通道
	NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;   //使能外部中断通道
	NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 1; //抢占优先级1
	NVIC_InitStruct.NVIC_IRQChannelSubPriority = 3;    //响应优先级3
	NVIC_Init(&NVIC_InitStruct);

	TIM_Cmd(TIM3,ENABLE);	  //使能定时器3
}

control.c (核心控制程序，此模块程序可供读者详细阅读)

#include "control.h"
int Target_velocity=50;  //设定速度控制的目标速度为50个脉冲每10ms

int TIM3_IRQHandler(void)
{
 if(TIM_GetFlagStatus(TIM3,TIM_FLAG_Update)==SET)
 {
     TIM_ClearITPendingBit(TIM3,TIM_IT_Update);       //===清除定时器3中断标志位
	 Encoder=Read_Encoder(2);                         //取定时器2计数器的值
     Led_Flash(100);                                  //LED闪烁
	 moto=Incremental_PI(Encoder,Target_velocity);    //===速度PID控制器
	 Xianfu_Pwm();
     Set_Pwm(moto);
 }
 return 0;
}
/**************************************************************************
函数功能：赋值给PWM寄存器
入口参数：PWM
返回  值：无
**************************************************************************/
void Set_Pwm(int moto)//赋值给PWM寄存器
{
 if(moto>0) AIN1=0,   AIN2=1;
 else       AIN1=1,   AIN2=0;
 PWMA=myabs(moto);
}
/**************************************************************************
函数功能：限制PWM赋值 
入口参数：无
返回  值：无
**************************************************************************/
 void Xianfu_Pwm(void) //限制幅度的函数
 {
     int Amplitude=7100;  //===PWM满幅是7200 限制在7100
	 if(moto<-Amplitude)  moto = -Amplitude;
	 if(moto>Amplitude)   moto =  Amplitude;
 }
/**************************************************************************
函数功能：绝对值函数
入口参数：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 (int Encoder,int Target)
{ 	
     float Kp=20,Ki=30;	
	 static int Bias,Pwm,Last_bias;
	 Bias=Encoder-Target;                //计算偏差
	 Pwm+=Kp*(Bias-Last_bias)+Ki*Bias;   //增量式PI控制器
	 Last_bias=Bias;	                 //保存上一次偏差 
	 return Pwm;                         //增量输出
}

三、位置闭环控制程序逻辑

【说明】下述程序中 control.c 最为重要，包含了速度闭环控制器的详细代码。其他程序模块供读者初始化参考。

main.c

u8 flag_Stop=1;     //停止标志位
int Encoder,Position=10000;        //编码器的脉冲计数
int moto;           //电机PWM变量
int main(void)
 {	
	 Stm32_Clock_Init(9);      //系统时钟设置
     delay_init();             //=====延时初始化
	 LED_Init();               //=====初始化与 LED 连接的硬件接口
	 uart_init(115200);        //=====初始化串口1
	 MOTO_Init();              //初始化控制电机所需的IO
	 pwm_Init(7199,0);         //初始化pwm输出
	 Encoder_Init_TIM2();      //初始化计数器（定时器）
	 TIM3_Int_Init(99,7199);   //10ms一次中断
	 while(1)
	 {
	   printf("Encoder:%d Position:%d \r\n",Encoder,Position);
	 }
 }

moto.c

void MOTO_Init(void)//初始化控制电机所需的IO
{
    GPIO_InitTypeDef GPIO_InitStruct;
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//PORTB12 13 14 15推挽输出
	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15;
	GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOB,&GPIO_InitStruct);	
}

void pwm_Init(u16 arr,u16 psc) //初始化pwm输出引脚
{	
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
	TIM_OCInitTypeDef TIM_OCInitStruct;
    GPIO_InitTypeDef GPIO_InitStruct;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1,ENABLE);  //使能定时器1时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE); //使能GPIOA的时钟
	
	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF_PP;         //复用输出
	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_11;   //PA8 PA11
	GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA,&GPIO_InitStruct);
	
	TIM_TimeBaseInitStruct.TIM_Period = arr;                     //设定计数器自动重装值 
	TIM_TimeBaseInitStruct.TIM_Prescaler  = psc;                 //设定预分频器
	TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
	TIM_TimeBaseInitStruct.TIM_ClockDivision = 0;                //设置时钟分割
	TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);              //初始化定时器
	
	TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM2;               //选择PWM2模式
	TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;   //比较输出使能
	TIM_OCInitStruct.TIM_Pulse = 0;                              //设置待装入捕获比较寄存器的脉冲值
	TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High;       //设置输出极性
	TIM_OC1Init(TIM1,&TIM_OCInitStruct);                         //初始化输出比较参数
	
	TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable); //CH1使能预装载寄存器
	 
	TIM_CtrlPWMOutputs(TIM1,ENABLE);                 //高级定时器输出必须设置这句
	
	TIM_ARRPreloadConfig(TIM1, ENABLE);              //使能TIM1在ARR上的预装载寄存器
	
	TIM_Cmd(TIM1,ENABLE);                            //使能定时器1
}

encoder.c

void Encoder_Init_TIM2(void)
{
  TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;  
  TIM_ICInitTypeDef TIM_ICInitStructure;  
  GPIO_InitTypeDef GPIO_InitStructure;
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);//使能定时器2的时钟
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);//使能PA端口时钟
	
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;	//端口配置
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
  GPIO_Init(GPIOA, &GPIO_InitStructure);					      //根据设定参数初始化GPIOB
  
  TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
  TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 预分频器 
  TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD-1; //设定计数器自动重装值
  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频：不分频
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_CenterAligned1;TIM向上计数  
  TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
	
  TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式3
	
  TIM_ICStructInit(&TIM_ICInitStructure); //把TIM_ICInitStruct 中的每一个参数按缺省值填入
  TIM_ICInitStructure.TIM_ICFilter = 10;  //设置滤波器长度
  TIM_ICInit(TIM2, &TIM_ICInitStructure);//根据 TIM_ICInitStruct 的参数初始化外设	TIMx
 
  TIM_ClearFlag(TIM2, TIM_FLAG_Update);//清除TIM的更新标志位
  TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);//使能定时器中断

  TIM_SetCounter(TIM2,10000);//设置TIMx 计数器寄存器值
  TIM_Cmd(TIM2, ENABLE); //使能定时器2
}

//中断处理函数为空，清除中断标志位后结束中断
void TIM2_IRQHandler(void)
{
 if(TIM_GetFlagStatus(TIM2,TIM_FLAG_Update)==SET)//溢出中断
 {
	 TIM_ClearITPendingBit(TIM2,TIM_IT_Update);//清除中断标志位 
 }
}

/**************************************************************************
函数功能：单位时间读取编码器计数
入口参数：定时器
返回  值：速度值
**************************************************************************/
int Read_Encoder(u8 TIMX)//读取计数器的值
{
  int Encoder_TIM;
	switch(TIMX)
	{
	    case 2:Encoder_TIM=(short)TIM2->CNT;  break;
		case 3:Encoder_TIM=(short)TIM3->CNT;  break;
		case 4:Encoder_TIM=(short)TIM4->CNT;  break;
		default: Encoder_TIM=0;
	}
  return Encoder_TIM;
}

timer.c

#include "timer.h"
/**************************************************************************
函数功能：定时中断初始化
入口参数：arr：自动重装值  psc：时钟预分频数 
返回  值：无
**************************************************************************/
void TIM3_Int_Init(u16 arr,u16 psc)
{
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
	NVIC_InitTypeDef NVIC_InitStruct;
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);
	
	TIM_TimeBaseInitStruct.TIM_Period = arr;     //重装载值
	TIM_TimeBaseInitStruct.TIM_Prescaler = psc;  //预分频系数
	TIM_TimeBaseInitStruct.TIM_ClockDivision =0; //时钟分割
	TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
	TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitStruct);
	
	TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE);  //使能定时器中断
	
	NVIC_InitStruct.NVIC_IRQChannel = TIM3_IRQn;   //使能外部中断通道
	NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;   //使能外部中断通道
	NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 1; //抢占优先级1
	NVIC_InitStruct.NVIC_IRQChannelSubPriority = 3;    //响应优先级3
	NVIC_Init(&NVIC_InitStruct);

	TIM_Cmd(TIM3,ENABLE);	  //使能定时器3
}

control.c (核心控制程序，此模块程序可供读者详细阅读)

#include "control.h"
int Target_position=11000;    //初始值是10000，目标值是11000
int TIM3_IRQHandler(void)
{
 if(TIM_GetFlagStatus(TIM3,TIM_FLAG_Update)==SET)
 {
     TIM_ClearITPendingBit(TIM3,TIM_IT_Update);   //===清除定时器1中断标志位
	 Encoder=Read_Encoder(2);                     //取定时器2计数器的值
     Led_Flash(100);                              //LED闪烁
	 moto=Position_PID(Encoder,Target_position);    //===位置PID控制器
	 Xianfu_Pwm();
     Set_Pwm(moto);
 }
 return 0;
}
/**************************************************************************
函数功能：赋值给PWM寄存器
入口参数：PWM
返回  值：无
**************************************************************************/
void Set_Pwm(int moto)//赋值给PWM寄存器
{
    if(moto>0) AIN1=0,   AIN2=1;
	else      AIN1=1,   AIN2=0;
	PWMA=myabs(moto);
}
/**************************************************************************
函数功能：限制PWM赋值 
入口参数：无
返回  值：无
**************************************************************************/
 void Xianfu_Pwm(void) //限制幅度的函数
 {
     int Amplitude=7100;  //===PWM满幅是7200 限制在7100
	 if(moto<-Amplitude)  moto = -Amplitude;
	 if(moto>Amplitude)   moto =  Amplitude;
 }
/**************************************************************************
函数功能：绝对值函数
入口参数：int
返回  值：unsigned int
**************************************************************************/
int myabs(int a) //取绝对值
{ 		   
	 int temp;
	 if(a<0)  temp=-a;  
	 else temp=a;
	 return temp;
}
/**************************************************************************
函数功能：位置式PID控制器
入口参数：编码器测量位置信息，目标位置
返回  值：电机PWM
根据位置式离散PID公式 
pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
e(k)代表本次偏差 
e(k-1)代表上一次的偏差  
∑e(k)代表e(k)以及之前的偏差的累积和;其中k为1,2,,k;
pwm代表输出
**************************************************************************/
int Position_PID (int Encoder,int Target)
{ 	
	 float Position_KP=80,Position_KI=0.1,Position_KD=500;
	 static float Bias,Pwm,Integral_bias,Last_Bias;
	 Bias=Encoder-Target;                                    //计算偏差
	 Integral_bias+=Bias;	                                 //求出偏差的积分
	 Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias);       //位置式PID控制器
	 Last_Bias=Bias;                                         //保存上一次偏差 
	 return Pwm;                                             //增量输出
}