isrs.c

AD converter 用于PIC开发dspic6014系列

	}
	// Now do final check to ensure control_output is not < 0
	// as this would cause large positive value to be loaded
	// into duty cycle registers!
	if (control_output < 0) control_output=0;

	// Now load calculated value into duty cycle registers
	// No need to disable writes to duty regsiters here
	// as we are not going to allow this code to be interrupted
	// (except by a fault) and assuming next pwm cycle starts after
	// registers are written.
	// Compensate calculated output due to fact that firing is inverted
	control_output=FULL_DUTY-control_output;		
	PDC1=(unsigned int)control_output;
	PDC2=(unsigned int)control_output;
	PDC3=(unsigned int)control_output;
	// Save current_error for use by D term next pass
	previous_current_error=current_error;
	// Clear D_TERM_DISABLE as this is only used for the first pass after
	// loop becomes active as don't have a valid previous error to base
	// D term calculation on.
	control_flags2.D_TERM_DISABLE=FALSE;
	return;
}

// This function is designed to determine the position of the rotor
// while the motor is turning but there is no current flowing.
// It looks for the sequence and timing between the rising edges of
// all three phase voltages. The sequence determines direction.
// The timing is used to set up the sensorless energisation
// This function will be called during starting and if lost during
// running.
// A rising edge of the 3 vph occur at the start of the following sectors
// Red = sector 5
// Yellow = sector 1
// Blue = sector 3	

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;
		
		return;
	}


	// Check for failure to acquire by monitoring retry counter
	// The retry counter is loaded first time acqusition starts
	// Every time same phase is detected as rising before another phase
	// the counter is decremented.
	// If the retry counter has reached zero and no other trip exists
	if ((retry_counter==0) && (trip_state==0))
	{
		run_state=FAULT;
		trip_state=FAILED_TO_START;
		control_flags.ACQUIRE2=FALSE;
		return;
	}
	// Note that all three sections of code below are similar in function but 
	// differ according to the particular sectors, directions etc
	// Red phase code is fully commented, Yellow and Blue are not

	/******************** START OF RED PHASE CODE **********************************/
	
	// To provide robust crossing detection, a similar scheme as used for zero
	// crossing is used except need a separate counter for each phase.
	// The level counter is only incremented if we see VPH below the acquire_threshold
	// and it has not reached the level_threshold. We are looking for a rising edge

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

	// If we have seen sufficient samples below the acquire_threshold
	if (level_count_red == (unsigned char)user_parameters_RAM[35])
	{
		// If previous sample and current sample are >= acquire_threshold
		// then we treat this as a valid rising edge
		if ((previous_vph_red >= vph_red_threshold) \
			&& (vph_red >= vph_red_threshold))
		{
			// Grab new timestamp from free-running counter (QEI in this case)
			new_edge_time=POSCNT;

			// If ACQUIRE2_RED is already set this means that have two edges
			// of RED before another phase which is not correct
			// If detected then reinitialize acquision and decrement retry_counter
			if (control_flags.ACQUIRE2_RED)
			{
				// The RETRY_FLAG is set so initialization code does not reload
				// the retry_counter
				control_flags2.RETRY_FLAG=TRUE;
				if (retry_counter)
					retry_counter--;
				// Setting this flag will force re-initialization next call
				control_flags.ADCCONFIG=TRUE;
				return;
			}
			// Reset level_counter so that VPH has to fall back beneath
			// acquire_threshold before rising check done again.
			level_count_red=0;

			// Now check to see if a previous phase's rising edge has already
			// detected. The sequence of the rising edges determines direction
			// and therefore also which sector has just been entered.
			if (control_flags.ACQUIRE2_BLUE==TRUE)
			{
				// Update the sector that rotor is in
				sector=5;
				// Update OVDCON with appropriate value
				OVDCON=SECTOR5_OVERRIDE;

				// Calculate times for 120 and 30 electrical degrees
				one_twenty_deg=new_edge_time-previous_edge_time;
				thirty_deg=one_twenty_deg/4;
				
				// Calculate period measurement so speed loop has valid data
				// and ensure don't try to re-acquire at too high a speed. 
				// The period measurement is 180 electrical degrees
				period_measurement=one_twenty_deg+(one_twenty_deg/2);
				// Reset stall counter to denote period has been calculated
				stall_counter=0;

				// To reach this section of code motor not windmilling and have detected a
				// second valid rising edge and so ought to be able to launch sensorless.
				// However, if going too fast, sensorless will get immediately lost due 
				// to enevitable error in threshold detection and because there may
				// not be time for sufficient samples of VPH to catch the zero crossing

				if (period_measurement > REACQUIRE_THRESHOLD) // If OK to run sensorless
				{	
					// To ensure sensorless can pick straight up need to
					// extrapolate backwards in time and set up previous timestamps
					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;
					// Setting the SWAP flag triggers the transition to sensorless
					control_flags.SWAP=TRUE;
					// Set run state to RUNNING in case this was a flying start
					run_state=RUNNING;
					// Force a Timer 2 interrupt event to start commutation
					IFS0bits.T2IF=TRUE;
					IEC0bits.T2IE=TRUE;
					ENABLE_FIRING;
				}
				else // Going too fast to re-acquire
				{
					// Force re-start of acqusition
					control_flags.ADCCONFIG=TRUE;
					// Also set ROTATION_CHECK flag
					// so that rotation timer can't timeout before
					// speed has fallen below where can re-acquire
					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 (period_measurement > REACQUIRE_THRESHOLD) // If OK to run sensorless
				{	
					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;
			}
			// Save timestamp just taken as it will be required when second rising
			// edge is detected to work out period etc.
			previous_edge_time=new_edge_time;
			// Set flag to indicate a valid rising edge has been detected on this phase
			control_flags.ACQUIRE2_RED=TRUE;
		}	
	}	
	/********************* END OF RED PHASE CODE *****************************************/

	/********************START OF YELLOW PHASE CODE **************************************/		
	if ((vph_yellow < vph_yellow_threshold) \
		&& (level_count_yellow < (unsigned char)user_parameters_RAM[35]))
	{
			level_count_yellow++;
	}

	if (level_count_yellow == (unsigned char)user_parameters_RAM[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 (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 (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;			
		}	
	}
	/********************* END OF YELLOW PHASE CODE **********************************/	
	
	/**********************START OF BLUE PHASE CODE **********************************/

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

	if (level_count_blue == (unsigned char)user_parameters_RAM[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 (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 (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;
		}	
	}
	/********************** END OF BLUE PHASE CODE *********************************/

	// Save current VPH samples for use next call of routine
	// Note that code above only returns before this point if an action is being taken 
	// on a rising edge detect.

	previous_vph_red=vph_red;
	previous_vph_yellow=vph_yellow;
	previous_vph_blue=vph_blue;

	return;
}