|
|
#include "control.h"
/**************************************************************************
作者：平衡小車之家
我的淘寶小店：shop114407458。taobao。com
**************************************************************************/
int Target_velocity=50; //設(shè)定速度控制的目標(biāo)速度為50個(gè)脈沖每10ms
int TIM1_UP_IRQHandler(void)
{
if(TIM1->SR&0X0001)//5ms定時(shí)中斷
{
TIM1->SR&=~(1<<0); //===清除定時(shí)器1中斷標(biāo)志位
Encoder=Read_Encoder(2); //===讀取編碼器的值，M法測(cè)速，輸出為每10ms的脈沖數(shù)
Led_Flash(100); //===LED閃爍;指示單片機(jī)正常運(yùn)行
Xianfu_Pwm(); //===PWM限幅
Moto1=Incremental_PI(Encoder,Target_velocity); //===速度PI控制器
Set_Pwm(Moto1); //===賦值給PWM寄存器
}
return 0;
}
/**************************************************************************
函數(shù)功能：賦值給PWM寄存器
入口參數(shù)：PWM
返回 值：無
**************************************************************************/
void Set_Pwm(int moto1)
{
if(moto1<0) AIN2=1, AIN1=0;
else AIN2=0, AIN1=1;
PWMA=myabs(moto1);
}
/**************************************************************************
函數(shù)功能：限制PWM賦值
入口參數(shù)：無
返回 值：無
**************************************************************************/
void Xianfu_Pwm(void)
{
int Amplitude=7100; //===PWM滿幅是7200 限制在7100
if(Moto1<-Amplitude) Moto1=-Amplitude;
if(Moto1>Amplitude) Moto1=Amplitude;
}
/**************************************************************************
函數(shù)功能：絕對(duì)值函數(shù)
入口參數(shù)：int
返回 值：unsigned int
**************************************************************************/
int myabs(int a)
{
int temp;
if(a<0) temp=-a;
else temp=a;
return temp;
}
/**************************************************************************
函數(shù)功能：增量PI控制器
入口參數(shù)：編碼器測(cè)量值，目標(biāo)速度
返回 值：電機(jī)PWM
根據(jù)增量式離散]]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代表增量輸出
在我們的速度控制閉環(huán)系統(tǒng)里面，只使用PI控制
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)
**************************************************************************/
int Incremental_PI (int Encoder,int Target)
{
float Kp=100,Ki=100;
static int Bias,Pwm,Last_bias;
Bias=Encoder-Target; //計(jì)算偏差
Pwm+=Kp*(Bias-Last_bias)+Ki*Bias; //增量式PI控制器
Last_bias=Bias; //保存上一次偏差
return Pwm; //增量輸出
}
|
評(píng)分
-
查看全部評(píng)分
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
