bldc.c

直流无刷电机的控制源代码

//	   		if (bldc_stepBufferFlag > 20)
//	   		{
//	   			ACTRA = 0x0DF3;		//5 24通
//	   			ACTRA &= 0xFF3F;
//	   			bldc_stepBufferFlag++;
//	   		}
//	   		else
	   			ACTRA = 0x0D3F;	//设置pwm5为低电平有效，pwm4强制为低 
	   	}
	   	else
			ACTRA = 0x0FFF; 
	}
	else
	{
		g_DutyCycle = 5000;
		g_DutyCycleLast = 0;
		g_DutyCycleNot = 0;
		g_lSum = 0;
		g_lUp = 0;
	    g_nErr = 0;
		g_lUi = 0;
		ACTRA = 0x0FFF;
		g_unSpeedFdb = 0; 
		g_unSpeedRef = 640;
	}
	
}  /* end of interrupt c_int02() */

void RstSystem(void)
{
#if (TARGET==F2407)
        disable_ints();           /* Make sure the interrupts are disabled   */
        IMR = 0x00;               /* Mask all interrupts                     */
        IFR = 0x00ff;             /* Clear any pending interrupts, if any    */
        PIRQR0 = PIRQR0 & 0x0fffe; /* Clear pending PDP flag */
        PIRQR2 = PIRQR2 & 0x0fffe; /* Clear pending PDP flag */

        EVAIFRA = EVAIFRA | 0x0001; /* Clear PDPINTA flag */
        EVBIFRA = EVBIFRA | 0x0001; /* Clear PDPINTB flag */

        asm("  CLRC   SXM ");         /* Clear signextension mode */
        asm("  CLRC   OVM ");         /* Reset overflow mode     */
        asm("  CLRC   CNF ");         /* Config block B0 to data memory */
        asm("  SPM    0   ");         /*  Set product mode at 0   */

        WSGR = WAIT_STATES;         /* Initialize Wait State Generator */
        SCSR1 = 0x0085;             /* Init SCSR1 */
        wdog.disable();           /* Vccp/Wddis pin/bit must be high */
        wdog.reset();             /* reset watchdog counter */

//        EVAIMRB=0x0004;        /* Enable the timer underflow interrupt */
		EVAIMRA=0x0200;    		/* Enable the timer1 underflow interrupt */
		
        EVAIFRA = 0XFFFF;      /* Clear all Group A interrupt flags */
        EVAIFRB = 0XFFFF;      /* Clear all Group B interrupt flags */
        EVAIFRC = 0XFFFF;      /* Clear all Group C interrupt flags */

        #if (REAL_TIME==TRUE)
        IMR = 0X0043;          /* En Int lvl 3 & 7 (T1 ISR)0044->0045 ygg int1打开 0X0045 */
        #endif /* (REAL_TIME==TRUE) */

        #if (REAL_TIME==FALSE)
        IMR = 0X0004;                /* En Int lvl 3   (T2 ISR) */
        #endif /* (REAL_TIME==TRUE)*/

#endif /* (TARGET==F2407) */

}   /* RstSystem(void) */


void interrupt phantom(void)
{
  static int phantom_count;
  phantom_count ++;
} /* phantom() */

/*------------------------------------------------------------------------------
    This function just provides a c-interface to the asm RTMON init function.
------------------------------------------------------------------------------*/

void rtmon_init(void)
{
    asm("       CALL    MON_RT_CNFG ");
}  /* rtmon_init() */

/*------------------------------------------------------------------------------
    This function derives time base from T2 underflow interrupt (i.e. period)
------------------------------------------------------------------------------*/

void time_base_init(void)
{
//        T2PR = 10000;  /* Initialize period register  10000对应采样频率为4kHz*/
///*
// * T2CON = 1001000001000000b
// * Operation is not affected by emulation suspend
// * Continuous up count mode
// * Enable timer operations
// */
//        T2CON = 0x9040;		//TICON==0X9040

} /* time_base_init() */

void evm_pwm_init(void)		//设置PWM_EN为低电平有效
{
    MCRC     = MCRC & 0XBFFF;        /* Select Secondary function IOPF6 2005-12-28 11:30*/
    PFDATDIR = PFDATDIR |0X4000;     /* Set IOPB6 as output        */
   	PFDATDIR = PFDATDIR & 0xFFBF; 	/* Set IOPB6 low/high, Enable/disable PWMEN低电平有效*/
    
    //------------------------------------------
    SCSR1|=0x0004;                       /* Turn EVA Clocks on          YES */
//      T1PR=p->period_max;                  /* Init Timer 1 period Register  =0x07d0=2000  YES */
        T1PR = 5000;			//PWM周期为8kHz，下溢产生中断 EVAIMRA=0x0200;更改vector.h中断设置
        T1CON=0x9040;	/* Init PWM Operation YGG  /Continuous-Up Count Mode/Enable timer operations     */
    							//PWM_INIT_STATE=0X9040		T1连续上(!)计数模式，使能Timer
		
        GPTCONA=0x0080;			//T1下溢触发ADC        
        ACTRA = 0x0FFF;     	//PWM端口强制为高电平，因为输出是低电平有效
        COMCONA=0xA200;			//使能比较，重新加载，使能PWM端口 
        
//      g_temp2=COMCONA;    	//查看第八位PDPINTx表示位，低电平关断PWM输出
//		GPTCONA=0x0240;			//YGG T12都下计数，不启动AD，T2下溢触发ADC    
    MCRA=MCRA|0x0fc0;    	//配置PWM1-6为PWM口，其它为IO口         
        
} /* evm_pwm_init */

void hall_pedal_init(void)		//初始化hall和踏板信号,根据硬件设置
{	
	MCRC = MCRC & 0XF5FF;	/*IOPF1=HALL_A,IOPF3=HALL_B,Select Secondary function as IO */
	MCRC = MCRC & 0XFFF1;	/*IOPE2=HALL_C,IOPE1=DIN_3,IOPE3=DIN_4,Select Secondary function as IO */
	
    PFDATDIR = PFDATDIR & 0XF5FF;	/* Set IOPF1,3 as Input    */
    PEDATDIR = PEDATDIR & 0xF1FF;	/* Set IOPE1,2,3 as Input   */
    
    //上下停车位初始化
    MCRA = MCRA & 0XFFDB;	/*IOPF1=HALL_A,IOPF3=HALL_B,Select Secondary function as IO */
    PADATDIR = PADATDIR & 0xDBFF;	//set IOPA5(up)，IOPA2(DN) as input    
    
    //pedal模拟ad输入初始化
    ADCTRL1=ADC_RESET_FLAG; 
    asm("  NOP ");
    asm("  NOP ");
    asm("  NOP ");
    ADCTRL1 = 0X0210;		//0010 0001 0001 0000	//0x0080//0X2210
    ADCTRL2 = 0X4700;		//0000 0111 0000 0000	//0X0700
    CALIBRATION = CALIBRATION_CONSTANT;
    MAXCONV = 0X0002;		//0    0    0000 0010 设置3个通道采样
    CHSELSEQ1 = 0X0410;		//0000 0100 0001 0000 三个通道  
}	//end of hall_pedal_init.

void read_hall(void)
{
	unsigned int temp = 0;
	temp = PFDATDIR & 0X0002;
	g_hall = temp << 1;		//HALL_A信号读出，在g_hall的倒数第三位上,后三位有效，其它位为0
	
	temp = PFDATDIR & 0X0008;
	g_hall |= temp >>2;		//HALL_B信号读出
	
	temp = PEDATDIR & 0X0004;
	g_hall |= temp >>2;		//HALL_C信号读出
	//-------------------------------------------
	temp = PEDATDIR & 0X0002;
	g_pedal_digital = temp >>1;	//DIN_3读出，在最低位，其它位为0
	
	temp = PEDATDIR & 0X0008;
	g_pedal_digital |= temp >>2;//DIN_3读出	
	
	//UP DN flag read
	temp = PADATDIR & 0x0004;	//read IOPA2(DN) as LSB
	g_unUpFlg = temp >> 2;
	
	temp = PADATDIR & 0x0020;	//read IOPA5(UP) as LSB
	g_unUpFlg |= temp >> 4;		//shit! 5-->4	
}	//end of read_hall.

void encoder_init(void)
{
	CAPCONA = 0x0000;	//0(00)0 0000 0000 0000B;
	T2CON = 0x1870;		//0X1870
	T2CNT = 0x0000;
	T2PR = 720;			//!
}
//增量式PID算法
void PIDInit(void)
{
	g_unSpeedRef = 0;	// 设定目标 Desired Value
	g_unKP = 4;			// 比例常数 Proportional Const
	g_unKI = 2;			// 积分常数 Integral Const
	g_unKD = 0;			// 微分常数 Derivative Const
	g_nErr = 0;			// 当前误差 g_nDeviation[0]
	g_nLastErr = 0;		// 上次误差 g_nDeviation[-1]
//	g_nPrevDeviation = 0;	// 上次误差 g_nDeviation[-2]
	for(i = 0;i <= 15;i++)
	{
		ADRESULT0[i] = 0;
		ADRESULT1[i] = 0;
	}
	
	i++;				//初始化为0			
//	j = &ADRESULT[0];		//取数组变量的头地址
//	j = ADRESULT;			//两句等效
	g_unLastPst = 0;
}

void speedFdb_calc(void)		//计算speed值，Q3格式
{
//	if (g_unLastPst > T2CNT)	//保证速度值大于0,时间刻度为(1/4k)ms,temp数据区间[0(0rpm)-0.6(200rpm)-15(5000rpm)]
//		g_temp1 = T2CNT + T2PR - g_unLastPst;
//	else
//		g_temp1 = T2CNT - g_unLastPst;
//		
//	g_unLastPst = T2CNT;		//保存新的位置数值
//
	if (i > 15)			//15-->2ms;
	{	
		i = 0;	
		if ((0 <= g_unLastPst) && (g_unLastPst < 690))
		{
			g_temp1 = (int)(T2CNT - g_unLastPst);
			g_unLastPst = T2CNT;
			
			if ((0 <= g_temp1) && (g_temp1 < 120))
				g_unSpeedFdb = g_temp1;						
		}
		else if ((690 <= g_unLastPst) && (g_unLastPst <= 720))
		{
			if (T2CNT < 30)
				g_temp1 = T2PR + T2CNT - g_unLastPst;
			else if (T2CNT >= g_unLastPst)
				g_temp1 = T2CNT - g_unLastPst;
			else
				g_temp1 = g_temp1;
				
			g_unSpeedFdb = g_temp1;	
			g_unLastPst = T2CNT;		
		}
		else
			g_errcode = 4;
		
		if (g_temp1 < -5)  
			g_errcode = 3;
		if (g_temp1 > 120)
			g_errcode = 5;	
	}
	else
		i++;		
		
	if ((T2CNT > 720) || (g_unSpeedFdb > 120))
	{
		g_unSpeedFdb = 120;
		g_errcode = 1;
	}
	if (g_unSpeedFdbMax < g_unSpeedFdb)
		g_unSpeedFdbMax = g_unSpeedFdb;
}

//void SpeedPID_calc(void)
//{
////	g_unSpeedFdb = (g_unSpeedFdb * 8 - 40) * 40 + 5120;	//g_unSpeedFdb[5--120]-->[40--960]
////	g_unSpeedFdbBig = g_unSpeedFdb * 320 + 3520;			//不会出现溢出
//	g_unSpeedFdbBig = (g_unSpeedFdb - 5) * 48 + 640;	//g_unSpeedFdb [5 120]
//	g_nErr = (int)(g_unSpeedRef - g_unSpeedFdbBig);
////	g_nErr = g_unSpeedRef;
//	g_lUp = (long)g_nErr * g_unKP;
//	
//	g_lUi = g_lUi + g_lUp / 512;
//	
//	if (g_lUimin > g_lUi)		//积分观察点
//		g_lUimin = g_lUi;
//	if (g_lUimax < g_lUi)
//		g_lUimax = g_lUi;
//		
//	if (g_lUi > 2500)
//		g_lUi = 2500;
//	if (g_lUi < -2500)
//		g_lUi = -2500;
//	
//	g_lSum = g_lUp + g_lUi;
//	
//	if (g_lSum < 0)			//PID控制器计算上下限限制
//		g_lSum = 0;
//	else if (g_lSum > 5000)
//		g_lSum = 5000;
//		
//	g_DutyCycleNot = (int)g_lSum;
//}
//
//void CRTLoop_init(void)
//{
////	int
//	
//}
//
//void Current_pid_calc(void)
//{
//	
//	
//	
//	if (g_lCRTSum >= 0)
//	{
//		CCWFlg = 1;
//		CWFlg = 0;
//	}
//	else
//	{
//		CCWFlg = 0;
//		CWFlg = 1;
//	}
//	
//	//--------------------------------------------
//	if (CCWOldFlg != CCWFlg)		//已经最精简啦
//	{
//		CCWOldFlg = CCWFlg;
//		CCWChangeFlg = 1;
//	}
//	else
//		CCWChangeFlg = 0;
//	
//}

//void currentFdb_calc(void)
//{
//	if (CCWFlg == 1)			//逆时针正转,*j为以0为基准的上偏移[0 -- 1.25*1024]
//	{
//		if (bldc_step <= 2)
//			g_unADCInput = adc0;			//输入大于1.25V
//		else if (bldc_step <= 4)
//			g_unADCInput = adc1;			//输入大于1.25V
//		else
//		{
//			g_unADCInput = adc0 + adc1 - 1.25 * 1024;	//输入小于1.25V + 1.25v
//			g_unADCInput = (1.25 * 1024 - g_unADCInput) + 1.25 * 1024	//
//		}
//		
//		g_unADCInput -= 1.25 * 1024;
//	}
//	else if (CWFlg == 1)		//顺时针反转,*j为以0为基准的上偏移[0 -- 1.25*1024]
//	{
//		if (bldc_step <= 2)
//		{
//			g_unADCInput = adc0;			//输入小于1.25V
//			g_unADCInput = 1.25 * 1024 - g_unADCInput;
//		}			
//		else if (bldc_step <= 4)
//		{
//			g_unADCInput = adc1;			//输入小于1.25V
//			g_unADCInput = 1.25 * 1024 - g_unADCInput;
//		}
//		else
//		{
//			g_unADCInput = adc0 + adc1 - 1.25 * 1024;	//输入大于1.25V + 0 
//			g_unADCInput -= 1.25 * 1024;
//		}			
//	}
//	else
//		g_errcode = 6;			//解决了多通道采样的规格统一化		
//		
//	//---------------------------------------------------------------------
//	if (CCWChangeFlg == 1)	//解决多次滤波给反馈电流计算带来的影响
//	{
//		for(i <= 8;i++)	//初始化为0	
//		{
//			ADRESULT0[i] = 0;
//			ADRESULT1[i] = 0;
//		}
//		
//		i = 0;				
//		ii = 0;
//		ADRESULT0Sum = 0;	
//		ADRESULT1Sum = 0;
//	}
//	else					//相等的话，进行滤波处理
//	{
//		ADRESULT0[i] = g_unADCInput;		//数据入口
//		ADRESULT0Sum += ADRESULT0[i];		
//		i++;
//		if(i >= 8)
//		{
//			i = 0;
//			ADRESULT1[ii] = ADRESULT0Sum / 8;
//			ADRESULT0Sum = 0;
//			ADRESULT1Sum += ADRESULT1[ii];
//			ii++;
//			if(ii >= 8)
//			{
//				ii = 0;
//				g_unADCOutput = ADRESULT1Sum / 8;
//				ADRESULT1Sum = 0;
//			}
//		}
//	}
//}	
//		if (ii < 8)
//		{
//			if (i < 8)		//0  --> 7
//			{
//				ADRESULT[i] = *j;
//				ADRESULTSum += ADRESULT[i];
//				i++;
//				j++;
//			}
//			else
//			{
//				i = 0;
//				ii++;
//			}
//		}
//		else
//		{
//			i = 0;
//			ii = 0;
//		}	
		
//	if (i <= 15)		//g_unSpeedFdb速度值>=0,时间刻度为(1/4k)*8=2ms,数据区间[0(0rpm)-5(200)-120(5000rpm)]
//	{							
//		g_unSpeedFdb += g_temp1;
//		g_unSpeedFdb -= *j;		//Q3格式：八次速度值相加而不除以八
//		*j++ = g_temp1;			//试用数组指针
//		i++;					//一定放在最后，i=0是开始
//	}
//	else
//	{
//		i = 0;
//		j = ADRESULT;
//	}