?? motorctrl.c

?? PID控制,包括算法,控制階梯列表,算法步進
?? C
?? 第 1 頁 / 共 2 頁
字號:
12 下一頁
//--------------------------------------------------------
//PID控制,包括算法,控制階梯列表,算法步進
//相關程序:ADC控制;
//--------------------------------------------------------
#include	"math.h"
#include	"stdlib.h"
#include	"..\inc\menu.h"
#include	"..\inc\MotorCtrl.h"

#define	SRSHIFT 8
#define SLSHIFT 5
#define CRSHIFT 8
#define CLSHIFT 5
#define LOWSPEED 30
#define XIANFU1 20
#define XIANFU 1000
#define ZEROSPEED 50

extern int ADRESULT[];
extern int spdgive;
extern unsigned int adc_js;
extern unsigned int ZHUNBEI_LED;
extern unsigned int YUNXING_LED;
extern unsigned int LINGSU_LED;
extern unsigned int GUZHANG_LED;
extern unsigned int qepCnt;

extern void disable();
extern void enable();
extern unsigned int iopbread();
extern unsigned int iopcread();
extern void adsoc();
extern void adstop();
extern void cfenable();
extern void cfdisable();
extern void leddisplay(unsigned int val);
extern void ledout(unsigned int val);
int Ctrldata[20];
unsigned int zero = 0;
unsigned int zeroCount = 0;
unsigned int digChange = 0;
unsigned int pointGive = 0;
unsigned int phasecnt;
float e_current[2];
float e_speed[2];
int zeroflag=0;

ioport unsigned port7;
ioport unsigned port9;

void pidinit();
void pidctrlopen();
void pidctrl();
void selftest();
void zerotest();
int fact();
void ext_ctrl();

//----------------------------------------------------------------
//	函數名稱：pidinit
//	輸入參數：無
//	輸出參數：無
//	功能描述：PID控制參數初始化，參與PI運算的參數清0
//----------------------------------------------------------------
void pidinit()
{
	e_speed[0]=0;
	e_speed[1]=0;
	e_current[0]=0;
	e_current[1]=0;
	Ctrldata[GEIDING]=0;
	Ctrldata[SENDOUT]=0;
	Ctrldata[CRTRF]=0;
	Ctrldata[ERRFLAG]=0;
	Ctrldata[LINGSU]=0;
	Ctrldata[WUCHAJIFEN]=0;
	Ctrldata[JIDIANQI]=0;
	port7=34000;
	return;
}

//----------------------------------------------------------------
//	函數名稱：pidopen
//	輸入參數：無
//	輸出參數：無
//	功能描述：開環控制，給定方式4時選擇
//			  公式:Ctrldata[SENDOUT]=30500-Ctrldata[GEIDING]*16
//----------------------------------------------------------------
void pidctrlopen()
{
	int dvolt;
	if((dvolt & BIT0) || (dvolt & BIT1))		//如果使能和運行關閉，清0
	{
		e_speed[0]=0;
		e_speed[1]=0;
		e_current[0]=0;
		e_current[1]=0;
		Ctrldata[GEIDING]=0;
		Ctrldata[SENDOUT]=0;
		Ctrldata[CRTRF]=0;
	}
	else		//否則開環控制
	{
		dvolt=Ctrldata[GEIDING]*16;	
		Ctrldata[SENDOUT]=dvolt;
		if(Ctrldata[SENDOUT]<0)
		{
			Ctrldata[SENDOUT] = 0;
		}
		if(Ctrldata[SENDOUT]>32000)
		{
			Ctrldata[SENDOUT]>32000;
		}
		dvolt=read_menudata(ERRSET);
		if(!Ctrldata[ERRFLAG])		//如果沒有故障，輸出
		{
			port7=30500-Ctrldata[SENDOUT];
		}
		else		//有故障，停止運行
		{
			port7=34000;
		}
	}
	return;
}

//----------------------------------------------------------------
//	函數名稱：pidctrl
//	輸入參數：無
//	輸出參數：無
//	功能描述：PID閉環控制
//		公式:速度環:Ctrldata[CRTRF]=Ctrldata[CRTRF]+e_speed[0]*(P+I/10)-espeed[1]*P
//			 		其中，e_speed在模擬給定方式中為Ctrldata[CHA];
//				    在數字給定方式中為(Ctrldata[GEIDING]-Ctrldata[SUDU])*4.
//           電流環:空載:Ctrldata[SENDOUT]=Ctrldata[SENDOUT]+(e_current[0]*(P1+I1/10)-e_current[1]*I1)/16
//			        加載:Ctrldata[SENDOUT]=Ctrldata[SENDOUT]+(e_current[0]*(P1+I1/10)-e_current[1]*I1)/32
//			        其中e_current=Ctrldata[CRTRF]-Ctrldata[DIANLIU].
//----------------------------------------------------------------
void pidctrl()
{
	int dvolt;
	int i,j;
	float microChange;
	float x1,x2;
	float Kca,Kcb;
	int speedMax;
	int speedMin;

	speedMax=read_menudata(SPDMAX);	
	speedMin=read_menudata(SPDMIN);
	
	dvolt=read_menudata(ERRSET);
	if(dvolt & BIT0)//使能關閉，則清0
	{
		e_speed[0]=0;
		e_speed[1]=0;
		e_current[0]=0;
		e_current[1]=0;
		Ctrldata[SENDOUT]=0;
	}
	else
	{
//-----------------------------速度環---------------------------------
		Kca=read_menudata(SPD_I);
		Kca/=10;
		Kca+=read_menudata(SPD_P);
		Kcb=read_menudata(SPD_P);
		switch(read_menudata(RFWAY))
		{
			case 0: x2=Ctrldata[SUDU];
					  break;
			case 1: x2=Ctrldata[SUDU];
					  break;
			case 2: dvolt=read_menudata(IRSET)*Ctrldata[DIANLIU]/10;
					Ctrldata[IRRF]=(Ctrldata[DIANYA]+dvolt)*4;
					x2=Ctrldata[IRRF];
					break;
			default: break;
		}
		x1=Ctrldata[GEIDING];
		e_speed[1]=e_speed[0];
		e_speed[0]=x1-x2;
		e_speed[0]*=4;
		
		if((read_menudata(SETWAY)==0)&&(pointGive==0))		//給定方式0，則用模擬差作為差量
		{
			j=Ctrldata[CHA];	//模擬差
			i=spdgive;
			if(i>=speedMax)
			{
				x2=spdgive;
				x1=e_speed[0] * Kca;
				x2=e_speed[1] * Kcb;
				x1=x1-x2;	//數字差
				microChange=x1;
			}
			else if(i<speedMin)
			{
				x2=spdgive;
				x1=e_speed[0] * Kca;
				x2=e_speed[1] * Kcb;
				x1=x1-x2;	//數字差
				microChange=x1;
			}
			else
			{
				e_speed[0]=j;
				x1=e_speed[0] * Kca;
				x2=e_speed[1] * Kcb;
				x1=x1-x2;	//變化量
				microChange=x1;
			}
		}
		else		//其他都是數字差量
		{
			x1=e_speed[0] * Kca;
			x2=e_speed[1] * Kcb;
			x1=x1-x2;	//數字差
			microChange=x1;
		}

		if((Ctrldata[GEIDING]<100) && (Ctrldata[GEIDING]>=90))		//以下為速度環分段，因為給定較小，進行放大
		{
			microChange*=2;
		}
		else if((Ctrldata[GEIDING]<90) && (Ctrldata[GEIDING]>=80))
		{
			microChange*=4;
	    }
	       else if((Ctrldata[GEIDING]<80) && (Ctrldata[GEIDING]>=60))
		{
			microChange*=6;
	    }
		else if(Ctrldata[GEIDING]<60)
		{
			microChange*=8;
		}		
		dvolt=(int)microChange;		//得到速度環最終的變化量
//--------------------------------電流環----------------------------
		Ctrldata[CRTRF]=Ctrldata[CRTRF]+dvolt;
		if(Ctrldata[CRTRF]<-4000)
		{
			Ctrldata[CRTRF]=-4000;
		}
		else if(Ctrldata[CRTRF]>16000)//1000*16
		{
			Ctrldata[CRTRF]=16000;
		}
		x2=Ctrldata[CRTRF];
		x1=Ctrldata[DIANLIU]*16;
//-------電流PI分段，以便在空載和加載時都能獲得良好的靜態和動態響應-------
		if(Ctrldata[DIANLIU]<30)		//空載PI
		{
			Kca=read_menudata(CRT_I1);
			Kca/=10;
			Kca+=read_menudata(CRT_P1);
			Kcb=read_menudata(CRT_P1);
		}
		else		//加載PI
		{
			Kca=read_menudata(CRT_I2);
			Kca/=10;
			Kca+=read_menudata(CRT_P2);
			Kcb=read_menudata(CRT_P2);
		}
		e_current[1]=e_current[0];
		e_current[0]=x2-x1;
		x1=e_current[0] * Kca;
		x2=e_current[1] * Kcb;
		x1=x1-x2;
		if(Ctrldata[DIANLIU]<30)
		{
			microChange=x1/32;
		}
		else
		{
			microChange=x1/64;
		}
		dvolt=(int)microChange;		//得到電流環最終的變化量
//----------------------------限幅-----------------------------------------
		if(dvolt > XIANFU)
			dvolt=XIANFU;
		else if(dvolt < -XIANFU)
			dvolt=-XIANFU; 
		Ctrldata[SENDOUT]=Ctrldata[SENDOUT]+dvolt;
		if(Ctrldata[SENDOUT]<0)
		{
			Ctrldata[SENDOUT]=0;
		}
		else if(Ctrldata[SENDOUT]>32000)
		{
			Ctrldata[SENDOUT]=32000;
		}
	}
//-----------------------------運行判斷--------------------------------------
	dvolt=read_menudata(ERRSET);
    if(pointGive==0)		
    {
		if((dvolt & BIT0) || (dvolt & BIT1))		//如為非點動給定，只有使能和運行同時按下，才正常輸出
		{
			e_speed[0]=0;
			e_speed[1]=0;
			e_current[0]=0;
			e_current[1]=0;
			Ctrldata[SENDOUT]=0;
			Ctrldata[CRTRF]=0;
		}
	}
	else
	{
		if(dvolt & BIT0)		//如為點動給定，則只要按下使能，即可正常輸出
		{
			e_speed[0]=0;
			e_speed[1]=0;
			e_current[0]=0;
			e_current[1]=0;
			Ctrldata[SENDOUT]=0;
			Ctrldata[CRTRF]=0;
		}
	}
//------------------------故障判斷---------------------------------------
	if(!Ctrldata[ERRFLAG])		//有故障，不輸出
	{
		port7=32000-Ctrldata[SENDOUT];
	}
	else
	{
		port7=34000;
	}
	return;
}

//--------------------------relay init----------------------------------
void relayinit()
{
	asm("	NOP");
	return;
}

//----------------------------------------------------------------
//	函數名稱：selftest
//	輸入參數：無
//	輸出參數：無
//	功能描述：故障保護及繼電器輸出，具體如下:
//			  BIT11：0：過壓
//			  BIT10：0：過流
//			  BIT9： 0：過載
//			  BIT8： 0：缺相
//			  BIT7： 0：失磁
//			  BIT6： 0：電機過熱
//            BIT5： 0：散熱器過熱
//----------------------------------------------------------------
void selftest()
{
	int temp,val;
	val=0;
	temp=0;
//-------------------------------------------------------			
	temp=(3*read_menudata(POWERMAX)/2);//過載功率為額定功率的150%
	if(read_menudata(PB_GUOZAI))//是否過載屏蔽
	{
		Ctrldata[ERRFLAG]=(Ctrldata[ERRFLAG]==GUOZAICODE)? 0:Ctrldata[ERRFLAG];
	}
	else if(read_menudata(POWER)>temp)//故障后只能復位
	{
		val |=BIT9;
		Ctrldata[ERRFLAG]=GUOZAICODE;//過載
	}
	temp=read_menudata(CRTMAX);//額定電流
	if(read_menudata(PB_GUOLIU))//是否過流屏蔽
	{
		Ctrldata[ERRFLAG]=(Ctrldata[ERRFLAG]==GUOLIUCODE)? 0:Ctrldata[ERRFLAG];
	}
	else if(read_menudata(CRT)>temp)//故障后只能復位
	{
		val |=BIT10;
		Ctrldata[ERRFLAG]=GUOLIUCODE;
	}
	temp=read_menudata(VOLTMAX);//額定電壓
	if(read_menudata(PB_GUOYA))//是否過壓屏蔽
	{
		Ctrldata[ERRFLAG]=(Ctrldata[ERRFLAG]==GUOYACODE)? 0:Ctrldata[ERRFLAG];
	}
	else if(read_menudata(VOLT)>temp)//故障后只能復位
	{
		val |=BIT11;
		Ctrldata[ERRFLAG]=GUOYACODE;//過壓
	}
//-------------------------缺相判斷---------------------
	if(phasecnt<30 || phasecnt> 45)//判斷為缺相（120ms內準確應為36次）
	{
		val |=BIT8;
		adsoc();//10-21燒寫后添加
	}
	phasecnt=0;
//-------------------------外部開關量輸入---------------
	temp=iopbread();
	temp &=0x00ff;
	val|=temp;
	write_menudata(ERRSET,val);
	
//------------開關量的保護功能實現----------------------
	val &=~(BIT14);//排除零速BIT14的影響
	val |=(BIT1+BIT0+BIT2+BIT3+BIT4);//排除外部按鍵的影響

	if(read_menudata(PB_QUEXIANG))val &=~BIT8;//故障屏蔽
	if(read_menudata(PB_SHICI)) val &=~BIT7;//失磁屏蔽
	if(read_menudata(PB_SRQGUORE)) val &=~BIT5;//散熱器過熱屏蔽
	if(read_menudata(PB_GUORE)) val |=BIT6;//電機過熱屏蔽
				
	if(val!=NORMAL)//檢測到故障
	{
		Ctrldata[ERRFLAG]|=0x0ffff;
	}
	else//故障消失后恢復正常
	{
		Ctrldata[ERRFLAG] &=0x0000;
	}
	if(Ctrldata[ERRFLAG])
	{
		if(read_menudata(JDQ2)==0)
		{
			Ctrldata[JIDIANQI] |=BIT1;
		}
		else
		{
			Ctrldata[JIDIANQI] &=(~BIT1);
		}
	}
	else
	{
		Ctrldata[JIDIANQI] &=(~BIT1);	
	}
	port9=Ctrldata[JIDIANQI];
	return;
}
//-------------------------------    -----------------------
//	函數名稱：zerotest
//	輸入參數：無
//	輸出參數：無
//	功能描述：0速繼電器輸出，
//----------------------------------------------------------
void zerotest()
{
    int temp;
//--------------------讀PB口狀態------------------------
	temp=iopbread();
	temp =temp&0x00ff;
	if(temp & BIT0)//使能關閉
	{
		Ctrldata[JIDIANQI] |=BIT0;
	}
	else
	{
		if(zeroCount>=150)//500ms累計給定和反饋小于一定值則0速
12 下一頁
?? 文件大小 6 K
?? 上傳用戶 forchwei
?? 所屬分類單片機開發
??? 相關標簽

#PID #控制 #算法 #步進
?? 快捷鍵說明

復制代碼 Ctrl + C
搜索代碼 Ctrl + F
全屏模式 F11
切換主題 Ctrl + Shift + D
顯示快捷鍵 ?
增大字號 Ctrl + =
減小字號 Ctrl + -
亚洲欧美第一页_禁久久精品乱码_粉嫩av一区二区三区免费野_久草精品视频

?? motorctrl.c

?? 快捷鍵說明
