u_funcs.c

无刷电机状态切换程序,包含启动、停止和刹车

#include "C:\Program Files\Microchip\work\ServoBLDC\h\u_var.h"
#include "C:\Program Files\Microchip\work\ServoBLDC\h\k_var.h"
#include "C:\Program Files\Microchip\work\ServoBLDC\h\p30f2010.h"
#include "C:\Program Files\Microchip\work\ServoBLDC\h\k_funcs.h"
int temp_AD = 0;
int	Stop_Ena = FALSE;


//采用算术平均滤波方法实现
//在函数Get_Pedal_Speed中采用加权滤波法
int AD_filter(void)
{
	unsigned int i,sum=0;
	for(i=0;i<AD_SAMPLE_NUM;i++)
		sum+=Pedal_AD_Value.AD_prePar[i];
	return sum/AD_SAMPLE_NUM;
}

/*******************************************************************************************
;*
;* Name: Get_Pedal_Speed
;* Description: Function of getting pedal speed
;* Function: This function gets pedal speed by sampling and filtering A/D 采用查询方式则屏蔽中断处理部分
;* Calls: None
;* Input: None
;* Outputs: None
;******************************************************************************************/
int Get_Pedal_Speed(void)
{
	int temp,Result;
	temp=AD_filter()-Pedal_AD_Start;
	if(temp<0)
		temp = 0;
	if(abs(temp-temp_AD)>20)
	{
		if(temp-temp_AD<=100)
			temp_AD = (temp >> 4) + ((15 * temp_AD) >> 4);	//Filter  (当前值的1/16加上前一个采样值的15/16)
		else
			temp_AD = temp;							//No need to filter
	}
	if (temp_AD <= Speed_Min)
		Result = Speed_Min;
	else											//Multiply the solpe parameter
		Result = Speed_Min + (temp_AD - Speed_Min) * Pedal_Slope;
    if(Result>=Speed_Max)
		Result = Speed_Max;


	return(Result);
}

//设置脚踏板标志位
//当脚踏板踩下时，置标志位Start_Signal_Flag
void Set_Pedal_Flag(void)
{
	if(START_MOTOR==1)		//如果马达脚踏板AD值大于域值，则置标志位
		Start_Signal_Flag=1;
	else Start_Signal_Flag=0;
}


void Clear_Task(void)
{
	Pedal_Denied = FALSE;
}

/***************************** Null Task ************************************************/
void Null(void)
{
	;
}
/***************************** Null Task End ********************************************/



/*
 *  FUNCTION: SetNullTask 
 *
 *  PARAMETERS: None
 *
 *  DESCRIPTION: (Added on 2005/12/17)
 *
 *  RETURNS: None
 *
 */
void SetNullTask(void)
{
	Current_Task.pTask = Null;
	Current_Task.RunMe = FALSE;				
	Current_Task.Running = FALSE;
}




/*;******************************************************************************************
*
;* Name: Motor_Init
;* Description: The initialization of motor parameters which are cases of kernel structure.
;* Function: This routine intializes motor parameters by assigning values to a copy of kernel
;* structure. 
;* Calls: None
;* Input: None
;* Outputs: None
;******************************************************************************************/ 
void Motor_Init(void)
{
	_SysMotor.PolePair = 2;
	_SysMotor.Rotation_Dir = CCW; 		//Forward  	ccw
	_SysMotor.Speed_Com = 0; 
	_SysMotor.RealV = 0; 
	_SysMotor.Current_HALL_Status = 0;
	_SysMotor.MotorStartFlag = 1;
	_SysMotor.SpeedMax = 6000;
	_SysMotor.Speed_Com_Min = 50-1;
	_SysMotor.Speed_Com_Max = 6000;
	ClrWdt();
}



/*;******************************************************************************************
*
;* Name: Task_Init
;* Description: The initialization of sewing tasks.
;* Function: This routine intializes sewing tasks which include intialization of preset 
;* parameters, operator parameters, running parameters, current task, current state and 
;* some other paramters. 
;* Calls: None
;* Input: None
;* Outputs: None
;******************************************************************************************/ 
void Task_Init(void)
{
	Clear_Task();
	Current_State = SewStop;
	SetNullTask();
	ClrWdt();
	
}


unsigned int MaxMinSpeed(int speed)
{
	if 	(speed<= Speed_Min)	
		speed = Speed_Min;
	if	(speed>= Speed_Max)
		speed = Speed_Max;
	return(speed);
}



/*******************************************************************************************
*
;* Name: Free_Sewing
;* Description: The Free_Sewing task.
;* Function: This routine implements the task adjusting motor speed according to the A/D sampling
;* value of pedal.
;* Calls: None
;* Input: None
;* Outputs: None
;******************************************************************************************/  
void Free_Sewing(void)
{
	if (!Current_Task.Running)					//First time of entering free sewing task 
	{
		Clear_Task();
		if (Current_State == SewStop)			//Soft start if first needle
		{
			if(Soft_Start_Flag)	//Soft Start initialization
			{
				Speed_Pedal = Speed_Soft_Start;
				Index_Soft = Stitch_Soft_Start;
			}
		}
		else
		{
			AD_Result = Get_Pedal_Speed() - Pedal_AD_Start;	//AD value of pedal signal, 1.2V is ignored 
			if(AD_Result<0)
				AD_Result = 0;
			Speed_Pedal = AD_Result;			//First pedal speed			
		}

		Current_State = SewRun;	
		Current_Task.Running = TRUE;
		Speed_Pedal = MaxMinSpeed(Speed_Pedal);
		SetMotorSpeed(Speed_Pedal);			//Motor start
		TimeVal1 = GetSysTime();			//Sampling time initialization
	}
	else			//Task is running
	{
		TimeVal2 = GetSysTime();
		TimeDiff =  TimeVal2.SysMSec-TimeVal1.SysMSec;
   		if(TimeDiff<0)
   			TimeDiff += 1000; 
   		if(TimeDiff>=1)						//Speed sampling time = 1ms
   		{
  			Current_State = SewRun;			//SewRun -> SewRun
			if (Test_Flag)	//Test state, no need to sample pedal speed
   				Speed_Pedal = Test_Speed - Test_Speed / 10; 
   			else
   			{
				Speed_Pedal = Get_Pedal_Speed();	//Obtain pedal speed by a/d sampling
			}
			Speed_Pedal = MaxMinSpeed(Speed_Pedal);
			TimeVal1 = TimeVal2;	
		}

		if((Soft_Start_Flag)&&(Index_Soft>0))	//Soft start routine
		{
			Index_Soft--;							//Compute soft stitches  
			ClrWdt();
		}
		else                    
		{
			SetMotorSpeed(Speed_Pedal); 
		}
	}
	
}


/*;******************************************************************************************
*
;* Name: Stop_Needle
;* Description: During free sewing, stop immediately when pedal released.
;* Calls: None
;* Input: None
;* Outputs: None
;******************************************************************************************/ 
void Stop_Needle(void)
{
	if(_SysMotor.RealV<=Stop_Middle_Speed+200)
	{
		Stop_Ena = TRUE;
		Current_Task.Running=FALSE;
		PWMCON1 = 0x0700;			// disable PWM outputs
  		OVDCON = 0x0000;			// overide PWM low.
	}
	else 
	{
		SetMotorSpeed(Stop_Middle_Speed);
		Current_Task.Running=FALSE;
		Stop_Ena = TRUE;
		PWMCON1 = 0x0700;			// disable PWM outputs
  		OVDCON = 0x0000;			// overide PWM low.
	}
}
	
/*
 *  FUNCTION: Check_State_Speed
 *
 *  PARAMETERS: None
 *
 *  DESCRIPTION: 状态检测函数:根据脚踏板状态/测试标志进行任务切换.
 *
 *  RETURNS: None
 *
*/
void Check_State_Speed(void)
{
	if (Test_Flag)
	{
		//测试过程中如检测到踏板前踩则跳出
		Set_Pedal_Flag();		//设置命令实际脚踏板标志
		if(Start_Signal_Flag==1)
		{
			Test_Flag = FALSE;
			Clear_Task();
			switch(Current_State)
			{
				case SewStop:	break; 
				default:	
					Current_Task.pTask = Stop_Needle;		
					Current_Task.RunMe = 1;
					Current_Task.Running = 0;
					break;
			}
		}
	}

	if (! Test_Flag)			// 读踏板信号
	{
		
		switch (Current_State)	
		{
			case SewStop:
				Current_Task.pTask = Free_Sewing;
				Current_Task.RunMe = 1;
				Current_Task.Running = 0;
				break;
	
			case SewRun:		
		   	//**************** 运行状态前踩继续自由缝
			if((Get_Pedal_Speed()>=MOTOR_START_ADVALUE)&&(START_MOTOR==1))
			{
				Current_Task.pTask = Free_Sewing;
				Current_Task.RunMe = 1;
				Current_Task.Running = 1;			// 保持原来的运行状态
			}
			else		//************** 运行状态松踏板进入停针任务
			{
				Current_Task.pTask = Stop_Needle;		
				Current_Task.RunMe = 1;
				Current_Task.Running = 0;
			}
			break;
			default:	break;	
		}

	}


}

void TimeDelay(unsigned int Minutes, unsigned int Seconds, unsigned int MillSeconds)
{
	if(Minutes>=60) return;
	if(Seconds>=60)return;
	if(MillSeconds>=1000)return;
	if(MillSeconds>0)
		Delay(MillSeconds*100);
	if(Minutes>0)
	{
		while(Minutes--)
			Delay(6000);
	}
	if(Seconds>0)
	{
		while(Seconds--)
			Delay(100);
	}
}