mc06005.c

一个可以运行在dsPIC30F微处理器上的电机控制代码。该代码以及后续说明完全使读者一看就明白。

*******************************************************/
#include <p30f6010.h>
#include "hardware.h"
#include "Def.h"

//-----Setup for device configuration bits---------
_FOSC(CSW_FSCM_OFF & XT_PLL4);
_FWDT(WDT_OFF);
_FBORPOR(PBOR_ON & BORV_42 & PWRT_64 & MCLR_EN);
//-------------------------------------------------

//-------------Global Variables-----------------------------------------
unsigned int Amplitude,Phase,Delta_Phase;//Amplitude---Voltage,Delta_Phase---Phase's increment
unsigned int Fsin = 0;					// Given frequence of motor
double Test_Fsin = 0.0;					// Actual frequence by measuring running motor
unsigned int i = 0,Period = 0;			// Save the total time from input capture 3
unsigned int PastCapture,ActualCapture;	// Save the captured value
unsigned int Capture_Buffer[9] = {0};	// Save the error

double Kp = 0.03,Ki = 0.003,Kd = 0.00;	// PID PID gains used by the controller
double PIDCoefficients[3] = {0.0};		// PID modified coefficients
double ControlDifference[3] = {0.0};	// Error(K),Error(K-1),Error(K-2)
double ControlOutput = 0.0;				// Controller output, used as a frequence output

//------------Function Declarations-------------------------------------
void COR_Init_ports(void);		// Initializatio of Ports used for motor control
void COR_Init_adc10(void);		// Initialization of ADC used for Speed demand
void COR_Init_timer3(void);		// Initialization for TIMER3 used as a timebase for the input capture channels
void COR_Init_timer1(void);		// Initialization for TIMER1 used for speed control
void COR_Init_pwm(void);		// Initialization for PWM at 16kHz, Center aligned, Complementary mode
void COR_Init_ic3(void);		// Initializes input captures
void COR_AP_ResetPowerModule(void);
void COR_AP_DelaySeconds(unsigned int);// Delay some seconds	
void COR_AP_RunMotor(void);		// This function initializes all variables and interrupts used for starting and 
								// running the motor
void COR_AP_StopMotor(void);	// This function clears all flags, and stops anything related to motor control, 
								// and also disables PWMs
extern void COR_AL_Modulation(void);	// This function will generate a sinewave with certain frequence
//-----------------------------------------------------------------------

void __attribute__((__interrupt__)) _T1Interrupt (void)
{
	int j;
	
	IFS0bits.T1IF = 0;	// Clear T1 interrupt flag
	//--------Period Calculation-------------------
	Period = 0;			// Clear variable Period
	for(j = 0;j < 8;j++)
		Period += Capture_Buffer[j];
	if(Period!=0)		// Motor is running
	{					// Calculate the frequence base on Period
		Test_Fsin = (double)RPM_POLE / Period;
	}
	else Test_Fsin = 0;	// Motor is stopped
	//--------------------------------------------

	//--------Speed Control(PID)------------------
	ControlDifference[0] = Fsin - Test_Fsin;
	ControlOutput += ControlDifference[0] * PIDCoefficients[0] + \
					 ControlDifference[1] * PIDCoefficients[1] + \
					 ControlDifference[2] * PIDCoefficients[2];
	ControlDifference[2] = ControlDifference[1];
	ControlDifference[1] = ControlDifference[0];
	
	if(ControlOutput > MAX_FSIN) ControlOutput = MAX_FSIN;
	else if(ControlOutput < MIN_FSIN) ControlOutput = MIN_FSIN;
	
	if(Test_Fsin >= MIN_FSIN && ControlOutput - Test_Fsin >= MIN_FSIN - 2) 
		ControlOutput = Test_Fsin;
	
	// Adjust Amplitude and Delta_Phase base on ControlOutput
	Amplitude = (unsigned int)(Kaf * (ControlOutput - MIN_FSIN) + 3470);
	Delta_Phase = (unsigned int)(ControlOutput * Fcoe);
	//--------------------------------------------
	
	return;
}


void __attribute__((__interrupt__)) _ADCInterrupt (void)
{
	unsigned int Ref_Sin = 0;
	
	IFS0bits.ADIF = 0;	// Clear ADC interrupt flag
	Ref_Sin = ADCBUF0;	// Gain value from the external Pot
	
	Fsin = MIN_FSIN + ((long)Ref_Sin * (MAX_FSIN - MIN_FSIN) >> 10);
	
	return;
}


void __attribute__((__interrupt__)) _PWMInterrupt (void)
{
	IFS2bits.PWMIF = 0;		// Clear PWM interrupt flag
	
	PWMCON2bits.UDIS = 1;	// Disable updata PDC1,PDC2 and PDC3
	COR_AL_Modulation();			// This ISR will be called for generating the sinewave
	PWMCON2bits.UDIS = 0;	// Enable updata PDC1,PDC2 and PDC3 

	return;
}


void __attribute__((__interrupt__)) _IC3Interrupt (void)
{
	IFS1bits.IC3IF = 0;		// Clear IC3 interrupt flag

	PastCapture = ActualCapture;
	ActualCapture = IC3BUF;	// Load Captured value form IC3BUF
	
	if(i == 0)
	{
		i++;
		return;
	}
	
	if(i<9)
	{						// Store the captured value for eight time
		Capture_Buffer[i - 1] = ActualCapture - PastCapture;
		i++;
	}
	else
	{
		i = 0;
	}
	
	return;
}


int main(void)
{
	COR_Init_ports();	// Inital Ports
	COR_Init_adc10();	// Inital ADC for sampling 
	COR_Init_pwm();		// Inital PWM 16Kz,center aligned
	COR_Init_ic3();		// Initial IC3 for Input Capture
	COR_Init_timer1();	// Initial TMR1 for 10ms periodic ISR
	COR_Init_timer3();	// Initialize TMR3 for timebase of capture
	
	COR_AP_ResetPowerModule();
	
	// This is a macro that enables the output buffers on the motor
	// control development board.
	ENABLE_FIRING;		// Enable PWM output
	ClrWdt();			// Clear watch dog

	// Main loop starts here.
	for(;;)
	{
		//This is a macro that allows user press a key and wait to release
		PRESS_DEBOUNCE(30);
		COR_AP_RunMotor();	// Run motor if push button is pressed and motor is stopped
		
		PRESS_DEBOUNCE(30);
		COR_AP_StopMotor();	// Stop motor if push button is pressed and motor is running
	}
	
	return;
}


void COR_AP_RunMotor(void)
{
	i = 0;
	
	ControlDifference[0] = 0;	// Error at K	(most recent)
	ControlDifference[1] = 0;	// Error at K-1
	ControlDifference[2] = 0;	// Error at K-2	(least recent)
	PIDCoefficients[0] = Kp + Ki + Kd;	// Modified coefficient for using PID
	PIDCoefficients[1] = -(Kp + 2 * Kd);// Modified coefficient for using
	PIDCoefficients[2] = Kd;			// Modified coefficient for using

	while(IC3CONbits.ICBNE)	// Clear IC3BUF
		IC3BUF;
		
	TMR1 = 0;	// Reset timer 1 for speed control
	TMR3 = 0;	// Reset timer 3 for period measurement
	
	IFS2bits.PWMIF = 0;		// Clear PWM interrupt flag
	IFS0bits.ADIF = 0;		// Clear ADC interrupt flag
	IFS1bits.IC3IF = 0;		// Clear IC3 interrupt flag
	IFS0bits.T1IF = 0;		// Clear T1 interrupt flag
	
	PTCONbits.PTEN = 1;		// Enable PWM Base Timer
	IEC2bits.PWMIE = 1;		// Enable PWM interrupr
	IEC1bits.IC3IE = 1;		// Enable interrupt on IC3
	IEC0bits.ADIE = 1;		// Enable interrupt on sampling
	IEC0bits.T1IE = 1;		// Enable T1 interrupt
	ADCON1bits.ADON = 1;	// Enable ADC sample
	T1CONbits.TON = 1;		// Turn on timer 1
	T3CONbits.TON = 1;		// Turn on timer 3
	
	return;
}


void COR_AP_StopMotor(void)
{
	PTCONbits.PTEN = 0;		// Turn off PWM
	IEC2bits.PWMIE = 0;		// Disable PWM interrupt
	
	IFS2bits.PWMIF = 0;		// Clear PWM interrupt flag
	IFS0bits.ADIF = 0;		// Clear ADC interrupt flag
	IFS1bits.IC3IF = 0;		// Clear IC3 interrupt flag
	IFS0bits.T1IF = 0;		// Clear T1 interrupt flag
	
	return;
}

/********************************************************************
	Initial the Ports according to necessary function.
*********************************************************************/
void COR_Init_ports(void)
{
	// Clear All Ports Prior to defining I/O
	PORTA = 0;
	PORTB = 0;
	PORTC = 0;
	PORTD = 0;
	PORTE = 0;
	PORTF = 0;
	PORTG = 0;
	
	// Ensure Firing is disabled
	DISABLE_FIRING;

	TRISD = 0xF7FF;
	TRISE = 0xFDFF;
	
	return;
}


void COR_Init_pwm(void)
{
	PTCON = 0x0002;			//Timebase on, runs in idle, no post or prescaler,
							// center aligned PWM
	PTPER = (FCY/FPWM - 1) >> 1;
							// this gives 16kHz PWM, in center aligned mode
	PWMCON1 = 0x0077;		// Phase#4 Firing Signals Not Used, all others independent
	PWMCON2 = 0x0F00;		// 1:16 Special Event Prescale, Fast Overrides, Updates enabled
	DTCON1 = 0x000F;		// Deadtime should be 2us		
	DTCON2 = 0x0000;				
	
	SEVTCMP = 0x0001;		// Setup special event trigger for ADC
	
	PDC1 = PTPER;			// Set all PWMs initially to 50%
	PDC2 = PTPER;
	PDC3 = PTPER;
	
	// Now clear any pending PWM or FAULT interrupts due to configue
	IFS2bits.PWMIF = 0;
	IFS2bits.FLTAIF =0;
	
	return;
}


void COR_Init_adc10(void)
{
	ADCON1 = 0x0066;
	ADCON2 = 0x0000;
	ADCON3 = 0x0003;
	
	// Setup A/D multiplexer CH0+ is AN7, CH0- is Vref-
	ADCHS = 0x0007;
	
	// Setup all A/D inputs as analog pins
	ADPCFG = 0xFFFF;
	
	ADPCFGbits.PCFG7 = 0;   // AN7 analog
	IFS0bits.ADIF = 0;		// Clear ADC Interrupt Flag
	
	return;
}


void COR_Init_ic3(void)
{
	TRISD |= 0x0400;		// Ensure IC1 capture RD8
	IC3CON = 0x0003;		/* Every 1th rising edge capture and inpterrupt
							   on every 1th capture event */ 
	IFS1bits.IC3IF = 0;		// Clear IC3 Interrupt Flag
	
	return;
}


void COR_Init_timer1(void)
{
	T1CON = 0x0020;			// internal Tcy/64 clock
	TMR1 = 0;
	PR1 = 1152;				// 100 ms interrupts for 20 MIPS
	
	return;
}


void COR_Init_timer3(void)
{
	T3CON = 0x0020;			// internal Tcy/64 clock
	TMR3 = 0;
	PR3 = 0xFFFF;
	 
	return;
}

void COR_AP_ResetPowerModule(void)
{
	FAULT_RESET=1;
	Nop();
	Nop();
	Nop();
	Nop();
	Nop();
	Nop();
	FAULT_RESET=0;
	
	return;
}


void COR_AP_DelaySeconds(unsigned int N)
{
	unsigned int j;
	
	while(N--)
	 	for(j=0;j < MILLISEC;j++);
	 	
	return;
}