isrs.c

用C语言写的用于微新公司数字信号控制器30F6014的

		}	
	}		
	previous_sample=vph;

	return;
}


//电流控制子程序
static void current_control(void)
{

	static long current_integral;
	static int previous_current_error;
	int current_error;
	long proportional_term;
	long derivative_term;
	int control_output;
	long temp;
	
 //检查以确定电流控制循环是否应该执行
	if ((run_state == STANDBY) || (control_flags.ACQUIRE2==TRUE) \
		|| (control_flags2.ROTATION_CHECK==TRUE))
	{
		current_integral=0;
		control_flags2.D_TERM_DISABLE=TRUE;
		return;
	}

	if ((run_state == STARTING) && ((user_parameters[40]==TRUE) \
		&& (control_flags2.WINDMILLING==FALSE)))
	{
		current_integral=0;
		control_flags2.D_TERM_DISABLE=TRUE;
		return;
	}

	if ((run_state == RUNNING) && ((user_parameters[1]==OPEN_VOLTS) \
		|| (user_parameters[1]==CLOSED_VOLTS)))
	{
		current_integral=0;
		control_flags2.D_TERM_DISABLE=TRUE;
		return;
	}

	//计算电流错误
	current_error=(current_demand+ibus_offset)-ibus;
	control_flags.VOLTS_LIMIT=FALSE;

	//计算比例和积分项
	proportional_term=((long)current_error*(long)iloop_p_gain);

	derivative_term=((long)(current_error-previous_current_error)\
							*(long)iloop_d_gain);

	if (control_flags2.D_TERM_DISABLE)
		derivative_term=0;

	//计算P+D项
	temp=proportional_term+derivative_term;

	if (temp > pos_duty_limit)
	{
		control_flags.VOLTS_LIMIT=TRUE;
		control_output=(pos_duty_limit/512)+ZERO_DUTY;
		current_integral=0;
	}
	else
	{
		if (temp < neg_duty_limit)
		{
			control_flags.VOLTS_LIMIT=TRUE;
			control_output=(neg_duty_limit/512)+ZERO_DUTY;
			current_integral=0;
		}
	}
	
	if (control_flags.VOLTS_LIMIT==FALSE)
	{

		//计算I 项
		current_integral+=((long)current_error * (long)iloop_i_gain);
		//把I项加到P+D中
		temp+=current_integral;

		if (temp > pos_duty_limit)
		{
			control_flags.VOLTS_LIMIT=TRUE;
			control_output=(pos_duty_limit/512)+ZERO_DUTY;
			current_integral = pos_duty_limit - proportional_term - derivative_term;
		}
		else
		{
			// If sum of P+I terms pushes you into negative limit
			if (temp < neg_duty_limit)
			{
				control_flags.VOLTS_LIMIT=TRUE;
				control_output=(neg_duty_limit/512)+ZERO_DUTY;
				current_integral=neg_duty_limit - proportional_term - derivative_term;
			}
			else
			{
				control_output=(temp/512)+ZERO_DUTY;
			}
		}
	}

	if (control_output < 0) control_output=0;
	//把计算值放入占空比寄存器
	control_output=FULL_DUTY-control_output;		
	PDC1=(unsigned int)control_output;
	PDC2=(unsigned int)control_output;
	PDC3=(unsigned int)control_output;
	previous_current_error=current_error;
	control_flags2.D_TERM_DISABLE=FALSE;
	return;
}


//确定转子位置子程序
static void acquire_position(void)
{

	static unsigned int previous_vph_red;
	static unsigned int previous_vph_yellow;
	static unsigned int previous_vph_blue;

	static unsigned char level_count_red;
	static unsigned char level_count_yellow;
	static unsigned char level_count_blue;

	static unsigned int previous_edge_time;
	static unsigned char retry_counter;
	unsigned int new_edge_time;
	unsigned int one_twenty_deg;
	unsigned int thirty_deg;
	

	if (control_flags.ADCCONFIG==TRUE)
	{
		control_flags.ADCCONFIG=FALSE;
		control_flags.ACQUIRE2_RED=FALSE;
		control_flags.ACQUIRE2_YELLOW=FALSE;
		control_flags.ACQUIRE2_BLUE=FALSE;
		previous_vph_red=0;
		previous_vph_yellow=0;
		previous_vph_blue=0;
		level_count_red=0;
		level_count_yellow=0;
		level_count_blue=0;
		previous_edge_time=0;
		if (control_flags2.RETRY_FLAG==FALSE)
			retry_counter=NO_RETRIES;
		control_flags2.WINDMILLING=FALSE;
		
		#ifdef DEBUG
			LATDbits.LATD15=1;
			indx=0;
		#endif
		return;
	}

	#ifdef DEBUG
		//data_log[indx]=vph_red;
		//data_log2[indx]=vph_yellow;
		//data_log3[indx]=vph_blue;
		//data_log4[indx]=POSCNT;
		if (indx<255)
			indx++;
	#endif


	if ((retry_counter==0) && (trip_state==0))
	{
		run_state=FAULT;
		trip_state=FAILED_TO_START;
		control_flags.ACQUIRE2=FALSE;
		return;
	}
	

	if ((vph_red < vph_red_threshold) \
		&& (level_count_red < (unsigned char)user_parameters[35]))
	{
			level_count_red++;
	}

	if (level_count_red == (unsigned char)user_parameters[35])
	{

		//如果上次采样和本次采样都大于acquire_threshold，则是有效的上升沿
		if ((previous_vph_red >= vph_red_threshold) \
			&& (vph_red >= vph_red_threshold))
		{

			new_edge_time=POSCNT;

			//如果ACQUIRE2_RED设定，初始化acquision且retry_counter减1
			if (control_flags.ACQUIRE2_RED)
			{
				#ifdef DEBUG
					LATDbits.LATD15=0;
				#endif
	
				control_flags2.RETRY_FLAG=TRUE;
				if (retry_counter)
					retry_counter--;
				control_flags.ADCCONFIG=TRUE;
				return;
			}

			level_count_red=0;

			//检查是否侦测到上一个相位的上升沿
			if (control_flags.ACQUIRE2_BLUE==TRUE)
			{
				#ifdef DEBUG
					LATDbits.LATD15=0;
				#endif

				sector=5;

				OVDCON=SECTOR5_OVERRIDE;

				//计算次数
				one_twenty_deg=new_edge_time-previous_edge_time;
				thirty_deg=one_twenty_deg/4;
				
				//计算测量周期
				period_measurement=one_twenty_deg+(one_twenty_deg/2);				
				stall_counter=0;

				//检测是否自转，如是先用电流控制马达，然后逐步减少输出频率直至稳定
				if (control_flags.DIR==BACKWARDS) 
				{

					//故意刹车
					sector=2;
					OVDCON=SECTOR2_OVERRIDE;

					control_flags2.WINDMILLING=TRUE;
					control_flags2.ROTATION_CHECK=FALSE;
					control_flags.ACQUIRE2=FALSE;
					//初始化换向周期为60度
					new_PR2=one_twenty_deg/2;

					windmilling_decel_rate=(BRAKING_PR2_LIMIT-new_PR2)/user_parameters[42];
					//反方向以使能量降低
					control_flags.DIR=FORWARDS;
					//使能Timer2中断，开始换向
					IFS0bits.T2IF=TRUE;
					IEC0bits.T2IE=TRUE;

					current_demand=windmilling_demand;
					ENABLE_FIRING;
					return;
				}
	
        //如果满足，则进入无刷运行 
				if (period_measurement > REACQUIRE_THRESHOLD)
				{	

					//设置时间间隙
					previous_timestamps[0]=new_edge_time - one_twenty_deg - thirty_deg;
					previous_timestamps[1]=new_edge_time - thirty_deg;
					previous_timestamps[2]=new_edge_time - one_twenty_deg + thirty_deg;
					previous_delta=one_twenty_deg/2;
					control_flags.ACQUIRE2=FALSE;					
					// 进入无刷运行模式
					control_flags.SWAP=TRUE;

					run_state=RUNNING;
					//设置Timer2中断事件，开始换向
					IFS0bits.T2IF=TRUE;
					IEC0bits.T2IE=TRUE;
					ENABLE_FIRING;
				}
				else 
				{					
					control_flags.ADCCONFIG=TRUE;

					control_flags2.ROTATION_CHECK=TRUE;
				}	
				return;
			}

			if (control_flags.ACQUIRE2_YELLOW==TRUE)
			{

				sector=2;
				OVDCON=SECTOR2_OVERRIDE;
				one_twenty_deg=new_edge_time-previous_edge_time;
				thirty_deg=one_twenty_deg/4;
				period_measurement=one_twenty_deg+(one_twenty_deg/2);
				stall_counter=0;
				if (control_flags.DIR==FORWARDS)	
				{
					sector=5;
					OVDCON=SECTOR5_OVERRIDE;
					control_flags2.WINDMILLING=TRUE;
					control_flags2.ROTATION_CHECK=FALSE;
					control_flags.ACQUIRE2=FALSE;
					new_PR2=one_twenty_deg/2;
					windmilling_decel_rate=(BRAKING_PR2_LIMIT-new_PR2)/user_parameters[42];
					control_flags.DIR=BACKWARDS;
					IFS0bits.T2IF=TRUE;
					IEC0bits.T2IE=TRUE;
					current_demand=windmilling_demand;
					ENABLE_FIRING;
					return;
				}

				if (period_measurement > REACQUIRE_THRESHOLD) 
				{	
					previous_timestamps[0]=new_edge_time - one_twenty_deg - thirty_deg;
					previous_timestamps[1]=new_edge_time - one_twenty_deg + thirty_deg;
					previous_timestamps[2]=new_edge_time - thirty_deg;
					previous_delta=one_twenty_deg/2;
					control_flags.ACQUIRE2=FALSE;
					control_flags.SWAP=TRUE;
					run_state=RUNNING;
					IFS0bits.T2IF=TRUE;
					IEC0bits.T2IE=TRUE;
					ENABLE_FIRING;
				}
				else
				{
					control_flags.ADCCONFIG=TRUE;
					control_flags2.ROTATION_CHECK=TRUE;
				}
				return;
			}

			previous_edge_time=new_edge_time;
			control_flags.ACQUIRE2_RED=TRUE;
		}	
	}	
	/********************* 红相代码结束 *****************************************/

	/********************黄相代码开始 **************************************/		
	if ((vph_yellow < vph_yellow_threshold) \
		&& (level_count_yellow < (unsigned char)user_parameters[35]))
	{
			level_count_yellow++;
	}

	if (level_count_yellow == (unsigned char)user_parameters[35])
	{
		if ((previous_vph_yellow >= vph_yellow_threshold) \
		&& (vph_yellow >= vph_yellow_threshold))
		{
			new_edge_time=POSCNT;
			
			if (control_flags.ACQUIRE2_YELLOW)
			{

				control_flags2.RETRY_FLAG=TRUE;
				if (retry_counter)
					retry_counter--;
				control_flags.ADCCONFIG=TRUE;
				return;
			}
			
			level_count_yellow=0;
			if (control_flags.ACQUIRE2_RED==TRUE)
			{

				sector=1;
				OVDCON=SECTOR1_OVERRIDE;
				one_twenty_deg=new_edge_time-previous_edge_time;
				thirty_deg=one_twenty_deg/4;
				period_measurement=one_twenty_deg+(one_twenty_deg/2);
				stall_counter=0;

				if (control_flags.DIR==BACKWARDS)
				{
					sector=4;
					OVDCON=SECTOR4_OVERRIDE;
					control_flags2.WINDMILLING=TRUE;
					control_flags2.ROTATION_CHECK=FALSE;
					control_flags.ACQUIRE2=FALSE;
					new_PR2=one_twenty_deg/2;
					windmilling_decel_rate=(BRAKING_PR2_LIMIT-new_PR2)/user_parameters[42];
					control_flags.DIR=FORWARDS;
					IFS0bits.T2IF=TRUE;
					IEC0bits.T2IE=TRUE;
					current_demand=windmilling_demand;
					ENABLE_FIRING;
					return;
				}

				if (period_measurement > REACQUIRE_THRESHOLD)
				{
					previous_timestamps[0]=new_edge_time - one_twenty_deg + thirty_deg;
					previous_timestamps[1]=new_edge_time - one_twenty_deg - thirty_deg;
					previous_timestamps[2]=new_edge_time - thirty_deg;
					previous_delta=one_twenty_deg/2;
					control_flags.ACQUIRE2=FALSE;
					control_flags.SWAP=TRUE;
					run_state=RUNNING;
					IFS0bits.T2IF=TRUE;
					IEC0bits.T2IE=TRUE;
					ENABLE_FIRING;
				}
				else
				{
					control_flags.ADCCONFIG=TRUE;
					control_flags2.ROTATION_CHECK=TRUE;
				}
				return;
			}

			if (control_flags.ACQUIRE2_BLUE==TRUE)
			{

				sector=4;
				OVDCON=SECTOR4_OVERRIDE;
				one_twenty_deg=new_edge_time-previous_edge_time;
				thirty_deg=one_twenty_deg/4;
				period_measurement=one_twenty_deg+(one_twenty_deg/2);
				stall_counter=0;

				if (control_flags.DIR==FORWARDS)
				{
					sector=1;
					OVDCON=SECTOR1_OVERRIDE;
					control_flags2.WINDMILLING=TRUE;
					control_flags2.ROTATION_CHECK=FALSE;
					control_flags.ACQUIRE2=FALSE;
					new_PR2=one_twenty_deg/2;
					windmilling_decel_rate=(BRAKING_PR2_LIMIT-new_PR2)/user_parameters[42];
					control_flags.DIR=BACKWARDS;
					IFS0bits.T2IF=TRUE;
					IEC0bits.T2IE=TRUE;
					current_demand=windmilling_demand;
					ENABLE_FIRING;
					return;
				}

				if (period_measurement > REACQUIRE_THRESHOLD)
				{
					previous_timestamps[0]=new_edge_time - thirty_deg;
					previous_timestamps[1]=new_edge_time - one_twenty_deg - thirty_deg;
					previous_timestamps[2]=new_edge_time - one_twenty_deg + thirty_deg;
					previous_delta=one_twenty_deg/2;
					control_flags.ACQUIRE2=FALSE;
					control_flags.SWAP=TRUE;
					run_state=RUNNING;
					IFS0bits.T2IF=TRUE;
					IEC0bits.T2IE=TRUE;
					ENABLE_FIRING;
				}
				else
				{
					control_flags.ADCCONFIG=TRUE;
					control_flags2.ROTATION_CHECK=TRUE;
				}
				return;
			}
			previous_edge_time=new_edge_time;
			control_flags.ACQUIRE2_YELLOW=TRUE;			
		}	
	}
	/********************* 黄相代码结束 **********************************/	
	
	/**********************蓝相代码开始 **********************************/

	if ((vph_blue < vph_blue_threshold) \
		&& (level_count_blue < (unsigned char)user_parameters[35]))
	{
			level_count_blue++;
	}

	if (level_count_blue == (unsigned char)user_parameters[35])
	{
		if ((previous_vph_blue >= vph_blue_threshold) \
			&& (vph_blue >= vph_blue_threshold))
		{
			new_edge_time=POSCNT;
			
			if (control_flags.ACQUIRE2_BLUE)
			{

				control_flags2.RETRY_FLAG=TRUE;
				if (retry_counter)
					retry_counter--;
				control_flags.ADCCONFIG=TRUE;
				return;
			}

			level_count_blue=0;
			if (control_flags.ACQUIRE2_YELLOW==TRUE)
			{

				
				sector=3;
				OVDCON=SECTOR3_OVERRIDE;
				one_twenty_deg=new_edge_time-previous_edge_time;
				thirty_deg=one_twenty_deg/4;
				period_measurement=one_twenty_deg+(one_twenty_deg/2);
				stall_counter=0;

				if (control_flags.DIR==BACKWARDS)
				{
					sector=0;
					OVDCON=SECTOR0_OVERRIDE;
					control_flags2.WINDMILLING=TRUE;
					control_flags2.ROTATION_CHECK=FALSE;
					control_flags.ACQUIRE2=FALSE;
					new_PR2=one_twenty_deg/2;
					windmilling_decel_rate=(BRAKING_PR2_LIMIT-new_PR2)/user_parameters[42];
					control_flags.DIR=FORWARDS;
					IFS0bits.T2IF=TRUE;
					IEC0bits.T2IE=TRUE;
					current_demand=windmilling_demand;
					ENABLE_FIRING;
					return;
				}

				if (period_measurement > REACQUIRE_THRESHOLD)
				{
					previous_timestamps[0]=new_edge_time - thirty_deg;
					previous_timestamps[1]=new_edge_time - one_twenty_deg + thirty_deg;
					previous_timestamps[2]=new_edge_time - one_twenty_deg - thirty_deg;
					previous_delta=one_twenty_deg/2;
					control_flags.ACQUIRE2=FALSE;
					control_flags.SWAP=TRUE;
					run_state=RUNNING;		
					IFS0bits.T2IF=TRUE;
					IEC0bits.T2IE=TRUE;
					ENABLE_FIRING;
				}
				else
				{
					control_flags.ADCCONFIG=TRUE;
					control_flags2.ROTATION_CHECK=TRUE;
				}
				return;
			}

			if (control_flags.ACQUIRE2_RED==TRUE)
			{

				sector=0;
				OVDCON=SECTOR0_OVERRIDE;
				one_twenty_deg=new_edge_time-previous_edge_time;
				thirty_deg=one_twenty_deg/4;
				period_measurement=one_twenty_deg+(one_twenty_deg/2);
				stall_counter=0;

				if (control_flags.DIR==FORWARDS)
				{
					sector=3;
					OVDCON=SECTOR3_OVERRIDE;
					control_flags2.WINDMILLING=TRUE;
					control_flags2.ROTATION_CHECK=FALSE;
					control_flags.ACQUIRE2=FALSE;
					new_PR2=one_twenty_deg/2;
					windmilling_decel_rate=(BRAKING_PR2_LIMIT-new_PR2)/user_parameters[42];
					control_flags.DIR=BACKWARDS;
					IFS0bits.T2IF=TRUE;
					IEC0bits.T2IE=TRUE;
					current_demand=windmilling_demand;
					ENABLE_FIRING;
					return;
				}

				if (period_measurement > REACQUIRE_THRESHOLD)
				{
					previous_timestamps[0]=new_edge_time - one_twenty_deg + thirty_deg;
					previous_timestamps[1]=new_edge_time - thirty_deg;
					previous_timestamps[2]=new_edge_time - one_twenty_deg - thirty_deg;
					previous_delta=one_twenty_deg/2;
					control_flags.ACQUIRE2=FALSE;
					control_flags.SWAP=TRUE;
					run_state=RUNNING;
					IFS0bits.T2IF=TRUE;
					IEC0bits.T2IE=TRUE;
					ENABLE_FIRING;
				}
				else
				{
					control_flags.ADCCONFIG=TRUE;
					control_flags2.ROTATION_CHECK=TRUE;
				}
				return;
			}
			previous_edge_time=new_edge_time;
			control_flags.ACQUIRE2_BLUE=TRUE;
		}	
	}
	/********************** 蓝相代码结束 *********************************/
  //保存当前得vph值，以便下次调用时使用
	previous_vph_red=vph_red;
	previous_vph_yellow=vph_yellow;
	previous_vph_blue=vph_blue;

	return;
}