medium_event.c

关于无传感器BLDC使用DSPIC现实的源码

		// (The actual limit is 0 as if volts too low just set
		// brake chopper switch on time to zero.) 
		// The use of -10 prevents chatter as otherwise if volts  
		// even 1 LSB too low, the integral term is cleared.
		if (voltage_error < -10)
		{
			voltage_integral=0;
			return;
		}
	}
	
	// Update Integral term here but this will be overwritten if
	// found to be in limit later on in code
	voltage_integral+=((long)voltage_error * (long)vloop_i_gain);
	temp=voltage_integral+proportional_term;

	// If sum of P+I terms pushes you into positive limit
	if (temp > POS_V_LIMIT)
	{
		voltage_integral = POS_V_LIMIT - proportional_term;
		return;
	}
	else
	{
		// If sum of P+I terms pushes you into negative limit
		// It is OK to use 0 as the negative limit here as
		// in steady state integral will be providing the
		// control effort and even if get a few LSBs of negative
		// error, voltage inegral is not cleared anyhow
		if (temp <= 0)
		{
			// Now calculate integrator limit and clamp
			voltage_integral = 0 - proportional_term;
			return;
		}
		else
		// Calculate control output based on both unclamped P and I terms
		{
			// Temp still contains unclamped P+I term so no need to recalculate
			// Load up PR3 with the On time of the switch
			PR3=(unsigned int)(temp/512);
			TMR3=0;
			// Set TIMER3 running so that when TMR3=PR3 switch turns off
			IFS0bits.T3IF=0;
			IEC0bits.T3IE=1;
		}
	}
	return;
}


void starting_code(void)
{
	unsigned int temp;
	unsigned long ltemp;
	static unsigned int rotation_timer=0;

	// First of all check to see whether motor is already turning
	// This is done by putting the system into acquire mode2
	// and by monitoring a countdown timer called rotation timer
	// If the system acquires it will automatically move to running
	// sensorless. If the timer reaches zero before the system has 
	// acquired then the normal lock and ramp routine is entered
	if (control_flags2.ROTATION_CHECK==TRUE)
	{
		DISABLE_INTERRUPTS;
		control_flags.ACQUIRE2=TRUE;
		control_flags.ADCCONFIG=TRUE;
		control_flags2.ROTATION_CHECK=FALSE;
		ENABLE_INTERRUPTS;
		rotation_timer=user_parameters[36];
		return;
	}
	
	if ((control_flags.ACQUIRE2==TRUE) && (rotation_timer))
	{
		rotation_timer--;
		return;
	}
	else
	{
		control_flags.ACQUIRE2=FALSE;
	}
	// Now check for windmilling (going opposite direction
	// to the demanded direction)
	// If windmilling then all we do is run open loop
	// decelerating motor with constant current demand
	// until near zero speed and then revert to normal
	// lock and ramp routine.
	if (control_flags2.WINDMILLING==TRUE)
	{
		if (new_PR2 < BRAKING_PR2_LIMIT)
		{
			new_PR2+=windmilling_decel_rate;
			return;
		}
		else
		{
			DISABLE_INTERRUPTS;
			control_flags2.WINDMILLING=FALSE;
			control_flags.DIR=user_parameters[0];
			IEC0bits.T2IE=0;
			control_flags.RAMP=FALSE;
			control_flags.LOCK1=FALSE;
			control_flags.LOCK2=FALSE;
			ENABLE_INTERRUPTS;
		}
	}
	// Are we in the ramping region of starting
	if (control_flags.RAMP==TRUE)
	{
		// The following section of code is done in assembly so that
		// can take advantage of the mul.us and div.sd instructions which
		// the compiler does not use due to ANSI C compliance
		// The calculations are:
		// new_speed= ramp_start_speed + (ramp_speed_delta*(starting_timer/ramp_time)^2)
		//	temp = ramp_start_demand + (ramp_demand_delta*starting_timer/ramp_time)
		// Note that the code is done within one asm instruction to ensure that
		// the compiler does not insert instructions when optimizing
		// First push w0-w5 onto the stack as the compiler may be using 
		// them to store intermediate results which are required later on
		// in the C code. This will be especially true if optimization is used

#define  V120	1	
		
#ifdef ORIG
		asm("	push.d	w0");
		asm("	push.d	w2");
		asm("	push.d	w4");
		asm("	mov.w	_starting_timer,w1");
		asm("	mov.w	_starting_timer,w0");
		asm("	mul.uu w0,w1,w4");
		asm("	mov.w _ramp_time,w2");
		asm("	repeat	#17");
		asm("	div.ud	w4,w2");
		asm("	mov.w _ramp_speed_delta,w1");
		asm("	mul.uu w0,w1,w4");
		asm("	repeat	#17");
		asm("	div.ud	w4,w2");
		asm("	mov.w	_ramp_start_speed,w1");
		asm("	add	w0,w1,w0");
		asm("	mov.w	w0,_new_speed");
		asm("	mov.w _ramp_demand_delta,w1");
		asm("	mov.w _starting_timer,w0");
		asm("	mul.us w0,w1,w4");
		asm("	mov.w _ramp_time,w2");
		asm("	repeat	#17");
		asm("	div.sd	w4,w2");
		asm("	mov.w	w0,_stemp");
		asm("	pop.d	w4");
		asm("	pop.d	w2");
		asm("	pop.d	w0");
#elif V120
                { unsigned long mul_result;
                  register unsigned int div_result asm("w0");
                  /* asm("instruction(s)" : outputs : inputs :
                                            clobbered registers ); */
 
                  asm volatile("mul.uu %1, %1, %0" : "=r"(mul_result) : 
                                            "r"(starting_timer));
                  asm volatile("repeat #17\n\t"
                      "div.ud %0, %1" : /* implied outputs */ : "r"(mul_result),
                                                                "r"(ramp_time) :
                                       "w0", "w1" );
                  asm volatile("mul.uu %1, %2, %0":"=r"(mul_result) : "r"(div_result),
                                                         "r"(ramp_speed_delta));
                  asm volatile("repeat #17\n\t"
                      "div.ud %0, %1" : /* implied outputs */ : "r"(mul_result),
                                                                "r"(ramp_time) :
                                       "w0", "w1" );
                  asm volatile("add %1, %2, %0" : "=r"(new_speed): "r"(ramp_start_speed),
                                                          "r"(div_result));
                  asm volatile("mul.us %1, %2, %0":"=r"(mul_result): "r"(starting_timer),
                                                        "r"(ramp_demand_delta));
                  asm volatile("repeat #17\n\t"
                      "div.ud %0, %1" : /* implied outputs */ : "r"(mul_result),
                                                                "r"(ramp_time) :
                                       "w0", "w1" );
                  stemp = div_result;
                }
#elif V130
                { unsigned long mul_result;
                  unsigned int div_result;

                  mul_result = __builtin_muluu(starting_timer, starting_timer);
                  div_result = __builtin_divud(mul_result, ramp_time);
                  mul_result = __builtin_muluu(div_result, ramp_speed_delta);
                  div_result = __builtin_divud(mul_result, ramp_time);
                  new_speed = div_result + ramp_start_speed;
                  mul_result =__builtin_mulus(starting_timer,ramp_demand_delta);
                  stemp = __builtin_divud(mul_result, ramp_time);
                }
#else
#error Which version
#endif
		temp=ramp_start_demand+stemp;
		
		// temp contains the new demand value for current ramp position
		// Now check to see if we are using voltage or current control
		if (user_parameters[40])
		{								
			// Compensate for inverted firing
			temp=FULL_DUTY-temp;
			// Disable updates of PWM duty registers to ensure
			// All 3 values get loaded together
			PWMCON2bits.UDIS=1;
			PDC1=temp;
			PDC2=temp;
			PDC3=temp;
			// Now enable updates
			PWMCON2bits.UDIS=0;
		}
		else
			current_demand=temp;
		
		// Calculate new period value based on new_speed value
		// number of motor poles and constants. Put it in ltemp
		ltemp=(COUNTER_RATE*20) / \
		((unsigned long)new_speed*(unsigned long)user_parameters[25]);

		// Check that a period overflow has not occurred before
		// actually writing the new value into new_PR2

		if (ltemp>65535)
			new_PR2=65535;
		else
			new_PR2=ltemp;
		// Now check to see which acquistion method is being used
		// If method 1 and the new step rate is < step rate where
		// zero crossing is enabled, then go ahead and set flag
		// so that zero crossings are checked during ramping.
		
		if ((user_parameters[43]==FALSE) && \
			(new_PR2<=acquire1_enable_rate) && \
			(control_flags.ACQUIRE1==FALSE))
		{
			control_flags2.ACQUIRE1_REQUEST=TRUE;
			acquire_counter=0;
		}

		// Now check to see if at end of ramp
		if (new_PR2 <=ramp_end_rate)
		{
			// Must ensure interrupt does not occur here
			// as otherwise can end up with ISR executing
			// with some flags correct and some not.
			// Also prefer to ensure all PWMs loaded up at
			// at same time to avoid glitches
			DISABLE_INTERRUPTS;
			control_flags.RAMP=FALSE;		
			DISABLE_FIRING;
			IEC0bits.T2IE = 0;

			// Now check to see which acquisition method being used
			// If using method 2 then set off acquire_position function
			// by setting appropriate flags
			if (user_parameters[43]==TRUE)
			{
				control_flags.ACQUIRE2=TRUE;
				control_flags.ADCCONFIG=TRUE;
				control_flags2.RETRY_FLAG=FALSE;
				run_state=RUNNING;
			}
			else 	// using Method1 should have already acquired before
			{		// end of ramp so enter fault condition
				run_state=FAULT;
				trip_state=FAILED_TO_START;
			}

			ENABLE_INTERRUPTS;
			return;
		}
	}	

	// If both LOCK flags are false then this is the first time we have entered
	// lock routines.

	if ((control_flags.LOCK1==FALSE) && (control_flags.LOCK2==FALSE))
	{
		if (control_flags.RAMP==FALSE)
		{
			starting_timer=0;
			control_flags.LOCK1=TRUE;
			// Now check to see if we are using voltage or current control
			if (user_parameters[40])
			{
				// Disable updates of PWM duty registers to ensure
				// All 3 values get loaded together
				PWMCON2bits.UDIS=1;
				// Compensate for inverted firing as FULL_DUTY gives
				// zero duty cycle.
				PDC1=(FULL_DUTY-hold1_demand);
				PDC2=PDC1;
				PDC3=PDC1;
				// Now enable updates
				PWMCON2bits.UDIS=0;
			}
			else
				current_demand=hold1_demand;

			// This write to OVDCON will only allow 2 of the 6 inverter
			// switches to actually fire so it does not matter that all
			// three duty cycle values were loaded with the demand
			OVDCON=SECTOR0_OVERRIDE;
			// Now enable firing
			ENABLE_FIRING;
		}
	}	
	
	// If at end of LOCK1 time
	if ((starting_timer >= user_parameters[2]) \
		&& (control_flags.LOCK1==TRUE))
	{
		starting_timer=0;
		control_flags.LOCK1=FALSE;
		control_flags.LOCK2=TRUE;

		// If using voltage control for starting
		if (user_parameters[40])
		{
			// Disable updates of PWM duty registers to ensure
			// All 3 values get loaded together
			PWMCON2bits.UDIS=1;

			// Compensate for inverted firing as FULL_DUTY gives
			// zero duty cycle.
			// Note that event though all three duty cycles are the 
			// same, the write to OVDCON below will only allow the 
			// correct switches to actually fire.
			PDC1=(FULL_DUTY-hold2_demand);
			PDC2=PDC1;
			PDC3=PDC1;
			// Now enable updates
			PWMCON2bits.UDIS=0;
		}
		else
			current_demand=hold2_demand;

		// Second energisation sector is direction dependent
		if (control_flags.DIR==FORWARDS)
			OVDCON=SECTOR1_OVERRIDE;
		else
			OVDCON=SECTOR5_OVERRIDE;
	}

	// If at end of LOCK2 time
	if ((starting_timer > user_parameters[3]) \
		&& (control_flags.LOCK2==TRUE))
	{
		control_flags.LOCK2=FALSE;
		control_flags.RAMP=TRUE;

		starting_timer=0;

		// Now energise motor with maximum
		// torque producive current which occurs
		// two sectors ahead. As have moved 1 sector
		// away from initial lock in correct direction
		// of rotation, this is the same sector whichever
		// direction we wish to rotate in.

		// If using voltage control for starting
		if (user_parameters[40])
		{
			// Disable updates of PWM duty registers to ensure
			// All 3 values get loaded together
			PWMCON2bits.UDIS=1;

			// Compensate for inverted firing as FULL_DUTY gives
			// zero duty cycle.
			PDC1=(FULL_DUTY-ramp_start_demand);
			PDC2=PDC1;
			PDC3=PDC1;
			// Now enable updates
			PWMCON2bits.UDIS=0;
		}
		else
			current_demand=ramp_start_demand;

		// Now check to see which acquistion method is being used
		// If method 1 and the initial ramp speed is > speed where
		// zero crossing is enabled, then go ahead and set flag
		// so that zero crossings are checked during ramping.
		if ((user_parameters[43]==FALSE) && \
			(ramp_start_rate<=acquire1_enable_rate))
		{
			control_flags2.ACQUIRE1_REQUEST=TRUE;
			acquire_counter=0;
		}
		
		// Write to the sector variable so that commutation ISR
		// will set up comutation correctly
		sector=3;
		// Load new_PR2 with initial step rate which will
		// be loaded into PR2 almost immediately as forcing
		// an interrupt of TMR2
		new_PR2=ramp_start_rate;
		IFS0bits.T2IF=TRUE;
		IEC0bits.T2IE=TRUE;
	}
	starting_timer++;
	return;
}