18motor.c

一款汽车马达控制的产品软件,使用PIC18C452芯片开发,能对汽车马达实现检测控制,可有效提高马达功率及减低功耗

//---------------------------------------------------------------------
//
//							 Software License Agreement
//
// The software supplied herewith by Microchip Technology Incorporated 
// (the 揅ompany? for its PICmicro?Microcontroller is intended and 
// supplied to you, the Company抯 customer, for use solely and 
// exclusively on Microchip PICmicro Microcontroller products. The 
// software is owned by the Company and/or its supplier, and is 
// protected under applicable copyright laws. All rights are reserved. 
//  Any use in violation of the foregoing restrictions may subject the 
// user to criminal sanctions under applicable laws, as well as to 
// civil liability for the breach of the terms and conditions of this 
// license.
//
// THIS SOFTWARE IS PROVIDED IN AN 揂S IS?CONDITION. NO WARRANTIES, 
// WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED 
// TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 
// PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. THE COMPANY SHALL NOT, 
// IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL OR 
// CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
//
//---------------------------------------------------------------------
//	File:		18motr2.c
//
//	Written By:		Stephen Bowling, Microchip Technology
//						
// This code implements a brush-DC servomotor using the PIC18C452 MCU.
// The code was compiled using the MPLAB-C18 compliler ver. 1.10
// The device frequency should be 20 MHz.
//
// The following files should be included in the MPLAB project:
//
//		18motr2.c		-- Main source code file
//		p18c452.lkr		-- Linker script file
//				
//		The following project files are included by the linker script:
//
//		c018i.o			-- C startup code
//		clib.lib			-- Math and function libraries
//		p18c452.lib		-- Processor library
//
//---------------------------------------------------------------------
//
// Revision History
//
// 2/15/02 -- PID compensator calculation modified to support 
// V2.00 of the MPLAB C-18 compiler.
// PID integral is now 32-bits to prevent integral overflow.
//
//---------------------------------------------------------------------

#include <p18c452.h>				// Register definitions
#include <stdlib.h>
#include <string.h>
#include <i2c.h>					// I2C library functions
#include <pwm.h>					// PWM library functions
#include <adc.h>					// ADC library functions
#include <portb.h>					// PORTB library function
#include <timers.h>					// Timer library functions

//---------------------------------------------------------------------
//Constant Definitions
//---------------------------------------------------------------------

#define	DIST	0					// Array index for segment distance
#define	VEL		1					// Array index for segment vel. limit
#define	ACCEL	2					// Array index for segment accel.
#define	TIME	3					// Array index for segment delay time
#define	INDEX	PORTBbits.RB0		// Input for encoder index pulse
#define	NLIM	PORTBbits.RB1		// Input for negative limit switch
#define	PLIM	PORTBbits.RB2		// Input for positive limit switch
#define	GPI		PORTBbits.RB3		// General purpose input
#define	MODE1	!PORTBbits.RB4		// DIP switch #1
#define	MODE2	!PORTBbits.RB5		// DIP switch #2
#define	MODE3	!PORTBbits.RB6		// DIP switch #3
#define	MODE4	!PORTBbits.RB7		// DIP switch #4
#define	SPULSE	PORTCbits.RC5	// Software timing pulse output
#define	ADRES	ADRESH				// Redefine for 10-bit A/D converter

//---------------------------------------------------------------------
// Variable declarations
//---------------------------------------------------------------------

const rom char ready[] = "\n\rREADY>";
const rom char badcmd[] = "\n\rERROR!";

char inpbuf[8];	// Input command buffer

unsigned char	
eeadr,			// Pointer to EEPROM address
firstseg,		// First segment of motion profile
lastseg,		// Last segment of motion profile
segnum,			// Current executing segment
parameter,		// Index to profile data
i,				// index to ASCII buffer
comcount,		// index to input string									
udata			// Received character from USART
;

struct {						// Holds status bits for servo
    unsigned	phase:1;		// Current phase of motion profile
    unsigned	neg_move:1;		// Backwards relative move
    unsigned	motion:1;		// Segment execution in progress
    unsigned	saturated:1;	// PWM output is saturated
    unsigned	bit4:1;
    unsigned	bit5:1;		
    unsigned	run:1;			// Enables execution of profile
    unsigned	loop:1;			// Executes profile repeatedly
} stat ;

int
dtime,							// Motion segment delay time
kp,ki,kd,						// PID gain constants
vlim, 							// Velocity limit for segment
mvelocity,						// Measured motor velocity
DnCount,						// Holds accumulated 'up' pulses
UpCount							// Holds accumulated 'down' pulses
;

union LNG									
{
long l;
unsigned long ul;
int i[2];
unsigned int ui[2];
char b[4];
unsigned char ub[4];
};

union LNG
temp,							// Temporary storage
accel,							// Segment acceleration value
error,							// PID error value
integral,               		// PID integral value
ypid,							// Holds output of PID calculation
velact,							// Current commanded velocity
phase1dist						// Half of segment distance
;

long              
position,      	   			// Commanded position.
mposition,					// Actual measured position.
fposition,					// Originally commanded position.
flatcount;     		   		// Holds the number of sample periods for which 
							// the velocity limit was reached in the first 
							// half of the move.  


#pragma udata segdata1 = 0x0100

int segment1[12][4];		// Holds motion segment values in data memory.
int segment2[12][4];		// 						"


#pragma udata

//---------------------------------------------------------------------
// Function Prototypes
//---------------------------------------------------------------------

void servo_isr(void);		// Does servo calculations
void isrhandler(void);		// Located at high interrupt vector

void DoCommand(void);		// Processes command input strings
void Setup(void);				// Configures peripherals and variables
void UpdPos(void);			// Gets new measured position for motor
void CalcError(void);		// Calculates position error
void CalcPID(void);			// Calculates new PWM duty cycle
void UpdTraj(void);			// Calculates new commanded position
void SetupMove(void);		// Gets new parameters for motion profile

// Writes a string from ROM to the USART
void putrsUSART(const rom char *data);

// ExtEEWrite and ExtEERead are used to read or write an integer value to the
// 24C01 EEPROM
																		
void ExtEEWrite(unsigned char address, int data);
int ExtEERead(unsigned char address);


//---------------------------------------------------------------------
// Interrupt Code
//---------------------------------------------------------------------

// Designate servo_isr as an interrupt function and save key registers

#pragma interrupt servo_isr save = PRODL,PRODH,temp

// Locate ISR handler code at interrupt vector

#pragma code isrcode=0x0008

void isrhandler(void)	// This function directs execution to the
{								// actual interrupt code										
_asm
goto servo_isr
_endasm
}


#pragma code
//---------------------------------------------------------------------
// servo_isr()
// Performs the servo calculations
//---------------------------------------------------------------------

void servo_isr(void)
{
SPULSE = 1;								// Toggle output pin for ISR code
											// timing
UpdTraj();								// Get new commanded position
UpdPos();								// Get new measured position
CalcError();							// Calculate new position error
CalcPID();

PIR1bits.TMR2IF = 0;					// Clear Timer2 Interrupt Flag.

SPULSE = 0;								// Toggle output pin for ISR code
}											// timing

//---------------------------------------------------------------------
// UpdTraj()
//	Computes the next required value for the next commanded motor
// position based on the current motion profile variables.  Trapezoidal
// motion profiles are produced.
//---------------------------------------------------------------------

void UpdTraj(void)
{

if(stat.motion && !stat.saturated)
	{
   if(!stat.phase)					// If in the first half of the move.
      {
      if(velact.ui[1] < vlim)	   // If still below the velocity limit
         velact.ul += accel.ul;  // Accelerate
         
                  
      else                      	// If velocity limit has been reached,
         flatcount++;				// increment flatcount.
      	           
      temp.ul = velact.ul;			// Put velocity value into temp
      									// and round to 16 bits
      if(velact.ui[0] == 0x8000)
      	{
      	if(!(velact.ub[2] & 0x01))
      		temp.ui[1]++;
      	}
      else
      
      if(velact.ui[0] > 0x8000) temp.ui[1]++;
                 
      phase1dist.ul -= (unsigned long)temp.ui[1];
   
      if(stat.neg_move)
      	position -= (unsigned long)temp.ui[1];
      else
         position += (unsigned long)temp.ui[1];
   
      if(phase1dist.l <= 0)		// If phase1dist is negative 
         								// first half of the move has 
      	stat.phase = 1;			// completed.
  	   }

   else									// If in the second half of the move,
      {									// Decrement flatcount if not 0.
      if(flatcount) flatcount--;
		
		else
		if(velact.ul)					// If commanded velocity not 0,
			{
			velact.ul -= accel.ul;	// Decelerate
						
			if(velact.i[1] < 0) 
				velact.l = 0;
			}
							
		else								// else								
			{
			if(dtime) dtime--;		// Decrement delay time if not 0.
			else
				{
				stat.motion = 0;		// Move is done, clear motion flag
				position = fposition;
				}
			}
	   	   
      temp.ul = velact.ul;			// Put velocity value into temp
      									// and round to 16 bits
      if(velact.ui[0] == 0x8000)
      	{
      	if(!(velact.ub[2] & 0x01))
      		temp.ui[1]++;
      	}
        
      else
      
      if(velact.ui[0] > 0x8000) temp.ui[1]++;
            
      if(stat.neg_move)				// Update commanded position
        	position -= (unsigned long)temp.ui[1];
      else
      	position += (unsigned long)temp.ui[1];
      }

	}										// END if (stat.motion)

else
	{
	if(stat.run && !stat.motion)	// If motion stopped and profile 
		{									// running, get next segment number
		if(segnum < firstseg) segnum = firstseg;
		if(segnum > lastseg)
			{	
			segnum = firstseg;		// Clear run flag if loop flag not set.
			if(!stat.loop) stat.run = 0;
			}
		else
			{
			SetupMove();				// Get data for next motion segment.
			segnum++;					// Increment segment number.
			}
		}	
	}
}

//---------------------------------------------------------------------
// SetupMove()
// Gets data for next motion segment to be executed
//---------------------------------------------------------------------

void SetupMove(void)
{
if(segnum < 12)								// Get profile segment data from
	{												// data memory.
	phase1dist.i[0] = segment1[segnum][DIST];
	vlim = segment1[segnum][VEL];
	accel.i[0] = segment1[segnum][ACCEL];
	dtime = segment1[segnum][TIME];
	}
else if(segnum < 24)
	{
	phase1dist.i[0] = segment2[segnum - 12][DIST];
	vlim = segment2[segnum - 12][VEL];
	accel.i[0] = segment2[segnum - 12][ACCEL];
	dtime = segment2[segnum - 12][TIME];
	}


phase1dist.b[2] = phase1dist.b[1];		// Rotate phase1dist one byte
phase1dist.b[1] = phase1dist.b[0];		// to the left.
phase1dist.b[0] = 0;
if(phase1dist.b[2] & 0x80) 				// Sign-extend value
   phase1dist.b[3] = 0xff;
else 
   phase1dist.b[3] = 0;

accel.b[3] = 0;								// Rotate accel one byte to 
accel.b[2] = accel.b[1];					// the left
accel.b[1] = accel.b[0];
accel.b[0] = 0;

temp.l = position;

if(temp.ub[0] > 0x7f)						// A fractional value is left 
	temp.l += 0x100;							// over in the 8 LSbits of 
temp.ub[0] = 0;								// position, so round position
													// variable to an integer value
position = temp.l;							// before computing final move
													// position.
fposition = position + phase1dist.l;	// Compute final position for 
   												// the move
if(phase1dist.b[3] & 0x80)					// If the move is negative,   
   {
   stat.neg_move = 1;						// Set flag to indicate negative 
   phase1dist.l = -phase1dist.l;			// move.
   }
else stat.neg_move = 0;						// Clear flag for positive move

phase1dist.l >>= 1;							// phase1dist holds total 
   							          		// move distance, so divide by 2
velact.l = 0;                          // Clear commanded velocity
flatcount = 0;									// Clear flatcount
stat.phase = 0;								// Clear flag:first half of move
if(accel.l && vlim)
stat.motion = 1;								// Enable motion
}

//---------------------------------------------------------------------
// UpdPos()
// Gets the new measured position of the motor based on values 
// accumulated in Timer0 and Timer1
//---------------------------------------------------------------------


void UpdPos(void)
{
// Old timer values are presently stored in UpCount and DnCount, so
// add them into result now.

mvelocity = DnCount;							
mvelocity -= UpCount;

//  Write new timer values into UpCount and DnCount variables.

UpCount = ReadTimer0();
DnCount = ReadTimer1();

// Add new count values into result.

mvelocity += UpCount;
mvelocity -= DnCount;

// Add measured velocity to measured position to get new motor
// measured position.

mposition += (long)mvelocity << 8;
}

//---------------------------------------------------------------------
// CalcError()
// Calculates position error and limits to 16 bit result
//---------------------------------------------------------------------

void CalcError(void)
{
temp.l = position;							// Put commanded pos. in temp
temp.b[0] = 0;									// Mask out fractional bits
error.l = temp.l - mposition;					// Get error
error.b[0] = error.b[1];							// from desired position and
error.b[1] = error.b[2];							// shift to the right to discard
error.b[2] = error.b[3];							// lower 8 bits.

if (error.b[2] & 0x80)							// If error is negative.
	{
	error.b[3] = 0xff;							// Sign-extend to 32 bits.

	if(error.l < -32768)
		error.l = -32768;						// Limit error to 16-bits.
	}

else												// If error is positive.
	{
	error.b[3] = 0x00;

	if(error.l > 32767)
		error.l = 32767;						// Limit error to 16-bits.
	}
}

//---------------------------------------------------------------------
// CalcPID()
// Calculates PID compensator algorithm and determines new value for
// PWM duty cycle
//---------------------------------------------------------------------

void CalcPID(void)
{
if(!stat.saturated)	// If output is not saturated,
   integral.l += error.l;  // modify the integral value.
   	

// Calculate the PID compensator.

ypid.l = (long)error.i[0]*(long)kp + integral.l*(long)ki + 
         (long)mvelocity*(long)kd;					


if(ypid.ub[3] & 0x80)					// If PID result is negative
	{
	if((ypid.ub[3] < 0xff) || !(ypid.ub[2] & 0x80))
		ypid.ul = 0xff800000;			// Limit result to 24-bit value
	}

else											// If PID result is positive
	{
	if(ypid.ub[3] || (ypid.ub[2] > 0x7f))
		ypid.ul = 0x007fffff;			// Limit result to 24-bit value
	}
	
ypid.b[0] = ypid.b[1];					// Shift PID result right to 
ypid.b[1] = ypid.b[2];					// get upper 16 bits.

stat.saturated = 0;						// Clear saturation flag and see
if(ypid.i[0] > 500)						// if present duty cycle output
	{											// exceeds limits.
	ypid.i[0] = 500;
	stat.saturated = 1;
	}
	
if(ypid.i[0] < -500)
	{
	ypid.i[0] = -500;
	stat.saturated = 1;
	}

ypid.i[0] += 512;							// Add offset to get positive 
												// duty cycle value.
												
SetDCPWM1(ypid.i[0]);					// Write the new duty cycle.
}


//---------------------------------------------------------------------