com.c

用ＴＭＳ３２０ＬＦ２４０７Ａ开发电机控制的源代码

/*********************************************************************
* Filename: Com.c                                                    *
*                                                                    *
* Author: Wu HongEn, Shandong University.                            *
*                                                                    *
* Last Modified: 06/01/03                                            *
*                                                                    *
* Description: .            *
*********************************************************************/


//Include header file
#include "String.h"
#include "Com.h" 
#include "motor.h"
#include "ev.h"

//Variable definition

/////////////////////////////
//initialization phase
int init_phase = PHASE_OPERATION; 
int call_status;
int config_status;


NP_UART_DEV uart_dev;

BYTE v_RXFIFOBuf[RXFIFO_MAX];
BYTE v_FIFOIdx;
BYTE v_FIFOCnts;
BOOL v_bFrameErr;
int v_ErrNo;

long intnum=0;
int idx=0;
int sei=0;

int PortControl=0;
int WriteSqu=0;
int BoardNum=0;
int PTimerReg;
Time_Struct Timer;

double motorCmd[4] = {0,0,0,0};

int speedA = 0;
int speedB = 0;
int speedC = 0;
int speedD = 0;

int speedABuf[16];
int speedBBuf[16];
int speedCBuf[16];
int speedDBuf[16];

int speedBufPCnt = 0;				//speed buffer pointer position

//Extern variable definition
extern int motorCtrlData[4];		//PID calculated motor control data

extern int motorAPos;
int motorAPosBak;

extern long motorBPos;
long motorBPosBak;

extern long motorCPos;
long motorCPosBak;

extern long motorDPos;
long motorDPosBak;

/*********************************************************************/
                                                           
void interrupt int950()
{ 
	int dataBuf;
	int i = 0;
	if(*PIVR==0x011)
	{
		uart_dev.isr_val = ISR_OFFSET&0X00FF;
		intnum++;

		if((uart_dev.isr_val & ISR_STATUS_ERROR)== ISR_STATUS_ERROR)
		{
			v_ErrNo = LSR_OFFSET&0X00FF;
		}
		else if((uart_dev.isr_val & ISR_DATA_AVALIABLE)==ISR_DATA_AVALIABLE)
		{   
			/* get counts of buffer */
			switch(init_phase)
			{
				case PHASE_OPERATION:
				{
					//Get command value from PC
					PTimerReg = Timer.ProcessT;
					v_FIFOCnts=N_ReadFIFOLevel(RECEIVE_FIFO)&0X00FF;
					for(v_FIFOIdx = 0; v_FIFOIdx < v_FIFOCnts; v_FIFOIdx ++)
					{
						v_RXFIFOBuf[v_FIFOIdx] = RHR_OFFSET&0X00FF;
					}
					
					//////////////////////////////////////////////////////////////////
					//Commad value from PC is 100000 times of real value
					//calculate real value then serve the motor 2 times, 
					//1 time increase half of the command
					//The other time is done in Timer 3 interrupt
					dataBuf = (v_RXFIFOBuf[0] & 0x00FF);
					dataBuf |= ((v_RXFIFOBuf[1] & 0x00FF)<<8);
					motorCmd[0] = dataBuf/200000.0;

					dataBuf = (v_RXFIFOBuf[2] & 0x00FF);
					dataBuf |= ((v_RXFIFOBuf[3] & 0x00FF)<<8);
					motorCmd[2] = dataBuf/200000.0;
					
					dataBuf = (v_RXFIFOBuf[4] & 0x00FF);
					dataBuf |= ((v_RXFIFOBuf[5] & 0x00FF)<<8);
					motorCmd[3] = dataBuf/200000.0;
					
					
					//Start Timer3 and clear Timer 1 counter
					*T3CNT = 0;
					*T3CON |= 0x0040;
					*T1CNT = 0;
					
					//Serve motor
					motorServe(MOTORA_ID, motorCtrlData[MOTORA_ID]);
					motorServe(MOTORB_ID, motorCtrlData[MOTORB_ID]);
					motorServe(MOTORC_ID, motorCtrlData[MOTORC_ID]);
					motorServe(MOTORD_ID, motorCtrlData[MOTORD_ID]);
					
					//Get Current position and then begin PID calculate
					QepACap();
					QepBCap();
					QepCCap();
					QepDCap();
					
					PID_Calculate(MOTORA_ID);
					PID_Calculate(MOTORB_ID);
					PID_Calculate(MOTORC_ID);
					PID_Calculate(MOTORD_ID);
					
					dataBuf = motorAPos - motorAPosBak;
					if(dataBuf > (SPINDLE_RESOLUTION/2))
					{
						speedABuf[speedBufPCnt] = dataBuf - SPINDLE_RESOLUTION;	
					}
					else if(dataBuf < -(SPINDLE_RESOLUTION/2))
					{
						speedABuf[speedBufPCnt] = dataBuf + SPINDLE_RESOLUTION;
					}
					else
					{
						speedABuf[speedBufPCnt] = dataBuf;
					}
					speedBBuf[speedBufPCnt] = motorBPos - motorBPosBak;
					speedCBuf[speedBufPCnt] = motorCPos - motorCPosBak;
					speedDBuf[speedBufPCnt] = motorDPos - motorDPosBak;
	
					speedA += (speedABuf[speedBufPCnt] - speedABuf[(speedBufPCnt+1) % 16]);
					speedB += (speedBBuf[speedBufPCnt] - speedBBuf[(speedBufPCnt+1) % 16]);
					speedC += (speedCBuf[speedBufPCnt] - speedCBuf[(speedBufPCnt+1) % 16]);
					speedD += (speedDBuf[speedBufPCnt] - speedDBuf[(speedBufPCnt+1) % 16]);
					
					motorAPosBak = motorAPos;
					motorBPosBak = motorBPos;
					motorCPosBak = motorCPos;
					motorDPosBak = motorDPos;
					
					speedBufPCnt ++;	
					if(speedBufPCnt >= 16)
					{
						speedBufPCnt = 0;
					}
					
					
					SendOutData();
				}
				break;

				case PHASE_CONFIG:
				{
					
					v_FIFOCnts=N_ReadFIFOLevel(RECEIVE_FIFO)&0X00FF;
					
					for(v_FIFOIdx = 0; v_FIFOIdx < v_FIFOCnts; v_FIFOIdx ++)
					{
						v_RXFIFOBuf[v_FIFOIdx] = RHR_OFFSET&0X00FF;
					}
					BoardNum = v_RXFIFOBuf[0];
					
					//	idx = v_RXFIFOBuf[2] & 0xff;
					Timer.ProcessT = v_RXFIFOBuf[2]*256 + v_RXFIFOBuf[1];
					//	idx = v_RXFIFOBuf[4] & 0xff;
					Timer.SendT = v_RXFIFOBuf[4]*256 + v_RXFIFOBuf[3];
					WriteSqu = v_RXFIFOBuf[5];
					
					PTimerReg = Timer.ProcessT;
					
					Timer.Step = 0;
					
					init_phase= PHASE_WAIT;
					
					PortControl |= CONFIG_OUT;
					port2000 = PortControl;
					
					for(v_FIFOIdx = 0; v_FIFOIdx < 3000; v_FIFOIdx ++)
					{
						v_FIFOIdx = v_FIFOIdx;
						
					}
					PortControl &= ~CONFIG_OUT;
					port2000 = PortControl;
				}
				break;

				case PHASE_WAIT:
				{
					v_FIFOCnts=N_ReadFIFOLevel(RECEIVE_FIFO)&0X00FF;
					for(v_FIFOIdx = 0; v_FIFOIdx < v_FIFOCnts; v_FIFOIdx ++)
					{
						v_RXFIFOBuf[v_FIFOIdx] = RHR_OFFSET&0X00FF;
					}
					if(v_RXFIFOBuf[0]==0xff)
						init_phase= PHASE_OPERATION;			
				}
				break;

				default:
				{
					//	*T1CON |= 0x0040;//start timer //168us
					
					v_FIFOCnts=N_ReadFIFOLevel(RECEIVE_FIFO)&0X00FF;
					for(v_FIFOIdx = 0; v_FIFOIdx < v_FIFOCnts; v_FIFOIdx ++)
					{
						v_RXFIFOBuf[v_FIFOIdx] = RHR_OFFSET&0X00FF;
					}
				}
			}
		}
		else if((uart_dev.isr_val & ISR_RCV_TIMEOUT)==ISR_RCV_TIMEOUT)
		{
			THR_OFFSET=intnum & 0xff;
		}
		
		uart_dev.shadowFCR |= FCR_FLUSH_RHR;
		FCR_OFFSET = uart_dev.shadowFCR;
		uart_dev.shadowFCR &= ~FCR_FLUSH_RHR;
		
		*XINT2CR = *XINT2CR & 0X8005;
	}
	else if(*PIVR==0x01)
	{
		init_phase = PHASE_CONFIG;

		PortControl |= SEND_ENABLE;
		portA000 = PortControl;
		for(v_FIFOIdx = 0; v_FIFOIdx < 128; v_FIFOIdx ++)
		{
			THR_OFFSET=v_FIFOIdx;
		}
		do{
			
		}
		while(N_ReadFIFOLevel(TRANSMIT_FIFO)&0X00FF);
		
		PortControl &= ~SEND_ENABLE;
		portA000 = PortControl;
		
		*XINT1CR = *XINT1CR & 0X8005;
		WriteSqu++;
	}
	
    enable();
    
}

/********************** C950 REGISTERS' OPERATION *********************/

WORD N_ReadDivisor ()
{
	WORD dlldlm;
	BYTE oldLCR;
	// store the current value of LCR and then set
	//the top bit to allow divisor latch access

	oldLCR = LCR_OFFSET;
	LCR_OFFSET = oldLCR | LCR_DL_ACCESS_KEY;
	// Construct the divisor word then restore LCR and return the value
	dlldlm = DLM_OFFSET << 8;
	dlldlm += DLL_OFFSET;
	LCR_OFFSET = oldLCR;

	return dlldlm;
}

/*************************************************************
* Function: N_WriteDivisor (WORD divisor)
* Description: Reads the ICR set register indexed by the index 
* parameter with value.
* Parameters: