?? seekline_pid.c
字號(hào):
/***************************PID算數(shù)有關(guān)**********************************************/
//unsigned int L_kp ;
//unsigned int A_kp ;
#define ANGLE_KPVALUE 1000
#define ANGLE_KIVALUE 0
//2
#define ANGLE_KDVALUE 0
//60
#define ANGLE_MAX 400<<10 //最大調(diào)節(jié)量輸出為正負(fù)400。
#define ANGLE_MIN -(400<<10)
#define ANGLE_Imax ( 10<<10 ) //位置PID，積分上限
#define ANGLE_Imin -( 10<<10 ) //位置PID，積分下限
#define ANGLE_DEADLINE 0X00 //速度PID，設(shè)置死區(qū)范圍
//////////////////////////////////////////////////////////////////////////////////
#define LOCA_KPVALUE 1600
#define LOCA_KIVALUE 3
#define LOCA_KDVALUE 60
#define LOCA_MAX 200<<10 //最大調(diào)節(jié)量輸出為正負(fù)200。
#define LOCA_MIN -(200<<10)
#define LOCA_Imax ( 10<<10 ) //位置PID，積分上限
#define LOCA_Imin -( 10<<10 ) //位置PID，積分下限
#define LOCA_DEADLINE 0X00 //速度PID，設(shè)置死區(qū)范圍
///////////////////////////////////////////////////////////////////////
typedef struct PID //定義數(shù)法核心數(shù)據(jù)
{
signed long angle_Ref; //位置PID，位移設(shè)定值
signed long angle_FeedBack; //位置PID，位移反饋值,當(dāng)前位置
signed long angle_PreError; //位置PID，前一次，位移誤差,ui_Ref - FeedBack
signed long angle_PreIntegral; //位置PID，前一次，位移積分項(xiàng)，ui_PreIntegral+ui
signed int angle_Kp; //位置PID，比例系數(shù)
signed int angle_Ki; //位置PID，積分系數(shù)
signed int angle_Kd; //位置PID，微分系數(shù)
signed long angle_PreU; //電機(jī)控制輸出值
signed long loca_Ref; //位置PID，位移設(shè)定值
signed long loca_FeedBack; //位置PID，位移反饋值,當(dāng)前位置
signed long loca_PreError; //位置PID，前一次，位移誤差,ui_Ref - FeedBack
signed long loca_PreIntegral; //位置PID，前一次，位移積分項(xiàng)，ui_PreIntegral+ui
signed int loca_Kp; //位置PID，比例系數(shù)
signed int loca_Ki; //位置PID，積分系數(shù)
signed int loca_Kd; //位置PID，微分系數(shù)
signed long loca_PreU; //電機(jī)控制輸出值
}PID;
PID skPID; // PID Control Structure
void SEEK_PIDInit (unsigned char seek_speed)
{
switch ( seek_speed )
{
case 0x00 :
{
skPID.angle_Kp = ANGLE_KPVALUE;//800; //
skPID.angle_Ki = ANGLE_KIVALUE;//20; //
skPID.angle_Kd = ANGLE_KDVALUE;//60;//
skPID.loca_Kp = LOCA_KPVALUE;//300;
skPID.loca_Ki = LOCA_KIVALUE;//2;
skPID.loca_Kd = LOCA_KDVALUE;//60;
break;
}
case 0x01 :
{
skPID.angle_Kp = 200;
skPID.angle_Ki = 2;
skPID.angle_Kd = 100;
skPID.loca_Kp = 200;
skPID.loca_Ki = 2;
skPID.loca_Kd = 100;
break;
}
case 0x02 :
{
skPID.angle_Kp = 150;
skPID.angle_Ki = 2;
skPID.angle_Kd = 100;
skPID.loca_Kp = 200;
skPID.loca_Ki = 2;
skPID.loca_Kd = 50;
break;
}
case 0x03 :
{
skPID.angle_Kp = 120;
skPID.angle_Ki = 2;
skPID.angle_Kd = 40;
skPID.loca_Kp = 120;
skPID.loca_Ki = 2;
skPID.loca_Kd = 40;
break;
}
case 0x04 :
{
skPID.angle_Kp = 100;
skPID.angle_Ki = 2;
skPID.angle_Kd = 30;
skPID.loca_Kp = 100;
skPID.loca_Ki = 2;
skPID.loca_Kd = 30;
break;
}
}
skPID.angle_Ref = 0 ; //位移設(shè)定值
skPID.angle_FeedBack = 0; //位移反饋值,當(dāng)前位置
skPID.angle_PreError = 0 ; //前一次，位移誤差,ui_Ref - FeedBack
skPID.angle_PreIntegral = 0 ; //前一次，位移積分項(xiàng)，ui_PreIntegral+ui
skPID.angle_PreU = 0 ;
skPID.loca_Ref = 0 ; //位移設(shè)定值
skPID.loca_FeedBack = 0; //位移反饋值,當(dāng)前位置
skPID.loca_PreError = 0 ; //前一次，位移誤差,ui_Ref - FeedBack
skPID.loca_PreIntegral = 0 ; //前一次，位移積分項(xiàng)，ui_PreIntegral+ui
skPID.loca_PreU = 0 ;
}
//PID參數(shù)設(shè)定
void SeekPIDPara (unsigned char seek_speed)
{
switch ( seek_speed )
{
case 0x00 :
{
skPID.angle_Kp = 300;
skPID.angle_Ki = 2;
skPID.angle_Kd = 60;
skPID.loca_Kp = 300;
skPID.loca_Ki = 2;
skPID.loca_Kd = 60;
break;
}
case 0x01 :
{
skPID.angle_Kp = 200;
skPID.angle_Ki = 2;
skPID.angle_Kd = 100;
skPID.loca_Kp = 200;
skPID.loca_Ki = 2;
skPID.loca_Kd = 100;
break;
}
case 0x02 :
{
skPID.angle_Kp = 150;
skPID.angle_Ki = 2;
skPID.angle_Kd = 100;
skPID.loca_Kp = 200;
skPID.loca_Ki = 2;
skPID.loca_Kd = 50;
break;
}
case 0x03 :
{
skPID.angle_Kp = 120;
skPID.angle_Ki = 2;
skPID.angle_Kd = 40;
skPID.loca_Kp = 120;
skPID.loca_Ki = 2;
skPID.loca_Kd = 40;
break;
}
case 0x04 :
{
skPID.angle_Kp = 100;
skPID.angle_Ki = 2;
skPID.angle_Kd = 30;
skPID.loca_Kp = 100;
skPID.loca_Ki = 2;
skPID.loca_Kd = 30;
break;
}
case 0x05 :
{
skPID.angle_Kp = 100;
skPID.angle_Ki = 2;
skPID.angle_Kd = 30;
skPID.loca_Kp = 100;
skPID.loca_Ki = 2;
skPID.loca_Kd = 30;
break;
}
case 0x06 :
{
skPID.angle_Kp = 100;
skPID.angle_Ki = 2;
skPID.angle_Kd = 30;
skPID.loca_Kp = 100;
skPID.loca_Ki = 2;
skPID.loca_Kd = 30;
break;
}
case 0x07 :
{
skPID.angle_Kp = 100;
skPID.angle_Ki = 2;
skPID.angle_Kd = 30;
skPID.loca_Kp = 100;
skPID.loca_Ki = 2;
skPID.loca_Kd = 30;
break;
}
case 0x08 :
{
skPID.angle_Kp = 100;
skPID.angle_Ki = 2;
skPID.angle_Kd = 30;
skPID.loca_Kp = 100;
skPID.loca_Ki = 2;
skPID.loca_Kd = 30;
break;
}
default :
{
skPID.angle_Kp = 100;
skPID.angle_Ki = 2;
skPID.angle_Kd = 30;
skPID.loca_Kp = 100;
skPID.loca_Ki = 2;
skPID.loca_Kd = 30;
}break;
}
}
//****************************************************************************************
signed int angle_PIDCalc( PID *pp )
{
signed long angle_error = 0,angle_derror = 0;
static long angle_Ditem = 0;
static unsigned int d_count = 100;
d_count++;
angle_error = pp->angle_Ref - pp->angle_FeedBack; // 偏差
if( ( angle_error < ANGLE_DEADLINE ) && ( angle_error > -ANGLE_DEADLINE ) ) //設(shè)置調(diào)節(jié)死區(qū)
{
}
else //執(zhí)行位置PID調(diào)節(jié)
{
angle_derror = angle_error - pp->angle_PreError; //計(jì)算微分項(xiàng)偏差
pp->angle_PreIntegral += angle_error; //存儲(chǔ)當(dāng)前積分偏差
pp->angle_PreError = angle_error; //存儲(chǔ)當(dāng)前偏差
if(pp->angle_PreIntegral > ANGLE_Imax) //積分修正,設(shè)定積分上下限，
pp->angle_PreIntegral = ANGLE_Imax;
else if(pp->angle_PreIntegral < ANGLE_Imin)
pp->angle_PreIntegral = ANGLE_Imin;
/*
else if( pp->angle_PreIntegral>0 && angle_error <0 ) //并于正負(fù)換向時(shí)清零
pp->angle_PreIntegral=0;
else if( pp->angle_PreIntegral<0 && angle_error >0 )
pp->angle_PreIntegral=0;
*/
else if( pp->angle_PreIntegral>0 && angle_derror <0 ) //變化趨勢(shì)改變，積分減半
pp->angle_PreIntegral >>= 1;
else if( pp->angle_PreIntegral<0 && angle_derror >0 )
pp->angle_PreIntegral >>= 1;
angle_Ditem *= 90; //加延時(shí)因子
angle_Ditem /= 100;
if(angle_derror != 0)
{
angle_Ditem += (angle_derror * pp->angle_Kd)/d_count;
d_count = 0;
}
pp->angle_PreU = pp->angle_Kp * angle_error + pp->angle_Ki * pp->angle_PreIntegral + angle_Ditem; //位置PID算法
if( pp->angle_PreU >= ANGLE_MAX ) //防止調(diào)節(jié)溢出
pp->angle_PreU = ANGLE_MAX;
else if( pp->angle_PreU <= ANGLE_MIN )
pp->angle_PreU = ANGLE_MIN;
}
return ( pp->angle_PreU >> 10 ); // 返回預(yù)調(diào)節(jié)占空比
}
//******************************************************************************************/
//****************************************************************************************
signed int loca_PIDCalc( PID *pp )
{
signed long loca_error = 0,loca_derror = 0;
static long loca_Ditem = 0;
static unsigned int d_count = 100;
d_count++;
loca_error = pp->loca_Ref - pp->loca_FeedBack; // 偏差
if( ( loca_error < LOCA_DEADLINE ) && ( loca_error > -LOCA_DEADLINE ) ) //設(shè)置調(diào)節(jié)死區(qū)
{
}
else //執(zhí)行位置PID調(diào)節(jié)
{
loca_derror = loca_error - pp->loca_PreError; //計(jì)算微分項(xiàng)偏差
pp->loca_PreIntegral += loca_error; //存儲(chǔ)當(dāng)前積分偏差
pp->loca_PreError = loca_error; //存儲(chǔ)當(dāng)前偏差
if(pp->loca_PreIntegral > LOCA_Imax) //積分修正,設(shè)定積分上下限，
pp->loca_PreIntegral = LOCA_Imax;
else if(pp->loca_PreIntegral < LOCA_Imin)
pp->loca_PreIntegral = LOCA_Imin;
/*
else if( pp->loca_PreIntegral>0 && loca_error <0 ) //并于正負(fù)換向時(shí)清零
pp->loca_PreIntegral=0;
else if( pp->loca_PreIntegral<0 && loca_error >0 )
pp->loca_PreIntegral=0;
*/
else if( pp->loca_PreIntegral>0 && loca_derror <0 ) //變化趨勢(shì)改變，積分減半
pp->loca_PreIntegral >>= 1;
else if( pp->loca_PreIntegral<0 && loca_derror >0 )
pp->loca_PreIntegral >>= 1;
loca_Ditem *= 90; //加延時(shí)因子
loca_Ditem /= 100;
if(loca_derror != 0)
{
loca_Ditem += (loca_derror * pp->loca_Kd)/d_count;
d_count = 0;
}
pp->loca_PreU = pp->loca_Kp * loca_error + pp->loca_Ki * pp->loca_PreIntegral + loca_Ditem; //位置PID算法
if( pp->loca_PreU >= LOCA_MAX ) //防止調(diào)節(jié)溢出
pp->loca_PreU = LOCA_MAX;
else if( pp->loca_PreU <= LOCA_MIN )
pp->loca_PreU = LOCA_MIN;
}
return ( pp->loca_PreU >> 10 );
} ?? 快捷鍵說(shuō)明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號(hào)
Ctrl + =
減小字號(hào)
Ctrl + -