motorctrl.c

PID控制,包括算法,控制阶梯列表,算法步进

		{
	   		zeroCount=0;
			if(zero<ZEROSPEED)
			{
				if(read_menudata(JDQ1)==0)
				{
					Ctrldata[JIDIANQI] |=BIT0;
				}
				else
				{
					Ctrldata[JIDIANQI] &=(~BIT0);
				}
				zeroflag=1;
			}
			else
			{
				Ctrldata[JIDIANQI] &=(~BIT0);
				zeroflag=0;
			}
			zero = 0;
		}
	
		else
		{
			zero+=read_menudata(SPDFB)+read_menudata(SPEEDSET);//累加
		}
	}
}
//----------------------------------------------------------------
//	函数名称：fact
//	输入参数：int a:需要定标的值
//					unsigned int b:定标用比例因子
//	输出参数：int :定标后值
//	功能描述：通过定标因子来改变输入参数的范围
//----------------------------------------------------------------
int fact(int a,unsigned int b)
{
	long x1,x2;
	x1=a;
	x2=read_menudata(b);
	switch(b)
	{
		case SPDFACT:		//速度定标
			x1=x1*x2/1024;
			break;
		case VOLTFACT:		//硬速定标
			x1=x1*x2/1024;
			break;
		case CRTFACT:		//转矩定标
			x1=x1*x2/1024;
			break;
		default:
			x1=0;
	}
	return((int)x1);
}

//----------------------------------------------------------------
//	函数名称：ext_ctrl
//	输入参数：无
//	输出参数：无
//	功能描述：选择给定方式具体如下:
//					如果非运行状态，点动按下，则为点动给定
//					如果为运行状态，则
//					给定方式为0则模拟给定，模拟差量
//								       1则模拟给定数字差量
//									2则数字给定，在这种方式下，
//									  若按下数字升或数字降，则为数字升降
//									3则硬速度给定
//									4则开环控制
//----------------------------------------------------------------
void ext_ctrl()
{
	int pbstate;
	int speedMax;
	int speedMin;
	int i,j;
	
	pbstate=read_menudata(ERRSET);
	speedMax=read_menudata(SPDMAX);
	speedMin=read_menudata(SPDMIN);
	//------------------点动给定----------------------
	if(pbstate & BIT1)//非运行状态
	{
		if(!(pbstate & BIT2))//点动给定按下
		{
		    pointGive=1;
			i=read_menudata(POINTSET);//写速度给定菜单
			if(i>=speedMax)
			{
				i=speedMax;
			}
			else if(i<=speedMin)
			{
				i=speedMin;
			}
			write_menudata(SPEEDSET,i);
			if(read_menudata(DIANXIE_EN)==0)//斜率为0则速度不变
			{
				Ctrldata[GEIDING]=i;
			}
			else
			{
				if(Ctrldata[GEIDING]<i)//升斜坡函数
				{
					j=read_menudata(POINTXIELV);//计算点动斜率
					if(j==0)
					{
						Ctrldata[GEIDING]=i;
					}
					else
					{
						Ctrldata[STEP]=1000/j;
						if((i-Ctrldata[STEP])>Ctrldata[GEIDING])
							Ctrldata[GEIDING]+=Ctrldata[STEP];//斜坡
						else
							Ctrldata[GEIDING]=i;
					}
				}
				else//稳定给定
				{
					Ctrldata[GEIDING]=i;
				}
			}
			i=Ctrldata[GEIDING];//回写最终给定菜单项
			write_menudata(SPDSET,i);
			return;
		}
		else//一切中间控制变量归零
		{
			e_speed[0]=0;
			e_speed[1]=0;
			e_current[0]=0;
			e_current[1]=0;
			Ctrldata[SENDOUT]=0;//30000;
			Ctrldata[CRTRF]=0;
			pointGive = 0;
			Ctrldata[GEIDING]=0;
		}
	}
	
	//---------------------数字给定-----------------------
	if(read_menudata(SETWAY)==2)//运行状态下-数字给定
	{
		i=read_menudata(KEYSET);//写速度给定菜单
		if(i>=speedMax)
		{
			i=speedMax;
		}
		else if(i<=speedMin)
		{
			i=speedMin;
		}
		if(i==0)
		{
			e_speed[0]=0;
			e_speed[1]=0;
			e_current[0]=0;
			e_current[1]=0;
			Ctrldata[SENDOUT]=0;//30000;
			Ctrldata[CRTRF]=0;
			Ctrldata[GEIDING]=0;		 
		}
		write_menudata(KEYSET,i);
		write_menudata(SPEEDSET,i);
				//键盘给定
		i=read_menudata(SPEEDSET);// 速度给定
		if(read_menudata(XIELV_EN)==0)
		{
			Ctrldata[GEIDING]=i;
		}
		else
		{
			if(Ctrldata[GEIDING]<i)//升斜坡函数
			{
				j=read_menudata(UPXIELV);//计算点动步进
					if(j==0)
					{
						Ctrldata[GEIDING]=i;
					}
					else
					{
						Ctrldata[STEP]=1000/j;
						Ctrldata[GEIDING]+=Ctrldata[STEP];//斜坡
						if(Ctrldata[GEIDING]>i)	
							Ctrldata[GEIDING]=i;
					}
				}
			else if(Ctrldata[GEIDING]>i)//降斜坡函数
			{	
				j=read_menudata(DOWNXIELV);//计算点动步进
					if(j==0)
					{
						Ctrldata[GEIDING]=i;
					}
					else
					{
						Ctrldata[STEP]=1000/j;
						Ctrldata[GEIDING]-=Ctrldata[STEP];//斜坡
						if(Ctrldata[GEIDING]<i)	
							Ctrldata[GEIDING]=i;
					}		
			}
			else//稳定给定
			{
				Ctrldata[GEIDING]=i;
			}
			disall();
		}
		i=Ctrldata[GEIDING];//回写最终给定菜单项
		write_menudata(SPDSET,i);
				//数字升
		if(!(pbstate & BIT3))//数字升 && 非模拟给定
		{
			i=read_menudata(DUPXIELV);//计算数字升步进
			Ctrldata[STEP]=(j==0)? 1000:1000/i;	
			i=read_menudata(KEYSET);
			if(i<speedMax)//斜坡函数
			{
				i+=Ctrldata[STEP];
				if(i>speedMax)
					i=speedMax;	
			}
			write_menudata(KEYSET,i);
			disall();
		}
		
				//数字降	
		else if(!(pbstate & BIT4))//数字降 && 非模拟给定
		{
			i=read_menudata(DDOWNXIELV);//计算数字降步进			
			Ctrldata[STEP]=(j==0)? 1000:1000/i;			
			i=read_menudata(KEYSET);
			if(i>speedMin)//斜坡函数
			{
				i-=Ctrldata[STEP];
			}
			write_menudata(KEYSET,i);
			disall();
		}
		if(i<0)
		{
			i=0;
			e_speed[0]=0;
			e_speed[1]=0;
			e_current[0]=0;
			e_current[1]=0;
			Ctrldata[SENDOUT]=0;//30000;
			Ctrldata[CRTRF]=0;
			Ctrldata[GEIDING]=0; 							
		}
	}

			//硬速度给定
	if(read_menudata(SETWAY)==3)//运行状态下-硬速度给定-无斜坡
	{
		i=fact(Ctrldata[HARDSPDSET],CRTFACT);
		if(i>=speedMax)
		{
			i=speedMax;
		}
		else if(i<=speedMin)
		{
			i=speedMin;
		}
		write_menudata(SPEEDSET,i);
		i=Ctrldata[GEIDING];//回写最终给定菜单项
		write_menudata(SPDSET,i);
		i=read_menudata(SPEEDSET);//速度给定
		if(i <= 0)
		{
			e_speed[0]=0;
			e_speed[1]=0;
			e_current[0]=0;
			e_current[1]=0;
			Ctrldata[SENDOUT]=0;//30000;
			Ctrldata[CRTRF]=0;
			Ctrldata[GEIDING]=0;
		}		
		else
		{
			i=read_menudata(SPEEDSET);// 速度给定
			if(read_menudata(XIELV_EN)==0)
			{
				Ctrldata[GEIDING]=i;
			}
			else
			{
				if(Ctrldata[GEIDING]<i)//升斜坡函数
				{
					j=read_menudata(UPXIELV);//计算步进
					if(j==0)
					{
						Ctrldata[GEIDING]=i;
					}
					else
					{
						Ctrldata[STEP]=1000/j;
						Ctrldata[GEIDING]+=Ctrldata[STEP];//斜坡
						if(Ctrldata[GEIDING]>i)	
							Ctrldata[GEIDING]=i;
					}
				}
				else if(Ctrldata[GEIDING]>i)//降斜坡函数
				{
					j=read_menudata(DOWNXIELV);//计算步进
					if(j==0)
					{
						Ctrldata[GEIDING]=i;
					}
					else
					{
						Ctrldata[STEP]=1000/j;
						Ctrldata[GEIDING]-=Ctrldata[STEP];//斜坡
						if(Ctrldata[GEIDING]<i)	
							Ctrldata[GEIDING]=i;
					}			
				}
				else//稳定给定
				{
					Ctrldata[GEIDING]=i;
				}
			}
		}
	}

			//开环控制
	if(read_menudata(SETWAY)==4)//开环控制
	{
		i=read_menudata(KEYSET);//写速度给定菜单
		if(i>=speedMax)
		{
			i=speedMax;
		}
		else if(i<=speedMin)
		{
			i=speedMin;
		}
		if(i==0)
		{
			e_speed[0]=0;
			e_speed[1]=0;
			e_current[0]=0;
			e_current[1]=0;
			Ctrldata[SENDOUT]=0;
			Ctrldata[CRTRF]=0;
			Ctrldata[GEIDING]=0;		 
		}
		Ctrldata[GEIDING]=i;
		write_menudata(KEYSET,i);
		write_menudata(SPEEDSET,i);
		write_menudata(SPDSET,i);
		disall();
	}

			//模拟给定，数字差
	if(read_menudata(SETWAY)==1)// 模拟给定
	{
		i=fact(spdgive,SPDFACT);
		if(i>=speedMax)
		{
			i=speedMax;
		}
		else if(i<=speedMin)
		{
			i=speedMin;
		}
		write_menudata(SPEEDSET,i);
		i=read_menudata(SPEEDSET);// 速度给定
		if(read_menudata(XIELV_EN)==0)
		{
			Ctrldata[GEIDING]=i;
		}
		else
		{
			if(Ctrldata[GEIDING]<i)//升斜坡函数
			{
				j=read_menudata(UPXIELV);//计算点动步进
					if(j==0)
					{
						Ctrldata[GEIDING]=i;
					}
					else
					{
						Ctrldata[STEP]=1000/j;
						Ctrldata[GEIDING]+=Ctrldata[STEP];//斜坡
						if(Ctrldata[GEIDING]>i)	
							Ctrldata[GEIDING]=i;
					}
			}
			else if(Ctrldata[GEIDING]>i)//降斜坡函数
			{
				j=read_menudata(DOWNXIELV);//计算点动步进
					if(j==0)
					{
						Ctrldata[GEIDING]=i;
					}
					else
					{
						Ctrldata[STEP]=1000/j;
						Ctrldata[GEIDING]-=Ctrldata[STEP];//斜坡
						if(Ctrldata[GEIDING]<i)	
							Ctrldata[GEIDING]=i;
					}			
			}
			else//稳定给定
			{
				Ctrldata[GEIDING]=i;
			}
		}
		if((read_menudata(SPEEDSET)==0) && (read_menudata(SPDSET)==0))
		{
			e_speed[0]=0;
			e_speed[1]=0;
			e_current[0]=0;
			e_current[1]=0;
			Ctrldata[SENDOUT]=0;//30000;
			Ctrldata[CRTRF]=0;
			Ctrldata[GEIDING]=0;
		}
		i=Ctrldata[GEIDING];//回写最终给定菜单项
		write_menudata(SPDSET,i);		
	}

			//模拟给定，模拟差，无斜坡
	if(read_menudata(SETWAY)==0)// 模拟给定
	{
		i=spdgive;
		if(i>=speedMax)
		{
			i=speedMax;
		}
		else if(i<=speedMin)
		{
			i=speedMin;
		}
		Ctrldata[GEIDING]=i;
		write_menudata(SPEEDSET,i);
		write_menudata(SPDSET,i);	
		if((read_menudata(SPEEDSET)==0) && (read_menudata(SPDSET)==0))
		{
			e_speed[0]=0;
			e_speed[1]=0;
			e_current[0]=0;
			e_current[1]=0;
			Ctrldata[SENDOUT]=0;
			Ctrldata[CRTRF]=0;
			Ctrldata[GEIDING]=0;
		}
	}
	return;
}