📄 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 + -