main.c

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

/************************************************************************
Project name: 	motorTest.pjt
File name: 		main.c
Author: 		武洪恩，胡天亮. 山东大学数控技术研究中心
Description:	The initializtion program and main function of ls7266.pjt
History:		(1)
*************************************************************************/

//include Header file
#include "f2407_c.h"
#include "pid.h"
#include "motor.h"
#include "ev.h"
#include "com.h"

//macro definition
#define DLE_SPI	0x20
#define CLOCK4	0x10	//clk4
#define CLOCK3	0x08	//clk3
#define CLOCK1	0x04	//clk1
#define CLOCK2	0x02	//clk2

//extern Variable definition
extern int motorCtrlData[4];		//PID calculated motor control data
extern int spiValToTrans;
extern float pidValue;				//PID increment value
extern double MaxSpeed;

extern PIDDataStruct pidDataAxisA;
extern PIDDataStruct pidDataAxisB;
extern PIDDataStruct pidDataAxisC;
extern PIDDataStruct pidDataAxisD;

extern int QepCNT;
extern int motorAPos;
extern long motorBPos;
extern long motorCPos;
extern long motorDPos;

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

extern int TimerPIntFlag;

//Variable definition


//屏蔽中断程序
void inline disable()
{
	asm(" setc INTM");
}
//开总中断程序
void inline enable()
{
	asm(" clrc INTM");
}
/***********************************************************
* Function: DspInit         							   *
* Input: 												   *
* Output: 												   *
* Description: Dsp initialization   					   *
* Resource allocating:
***********************************************************/
void inline DspInit()
{
	asm(" setc SXM");			//符号位扩展有效
	asm(" clrc OVM");			//累加器中结果正常溢出
	asm(" clrc CNF");			/* B0被配置为数据存储空间*/	
/* Configure the System Control and Status registers */
    *SCSR1 = 0x83FE;
/*
 bit 15        1:      reserved
 bit 14        0:      CLKOUT = CPUCLK
 bit 13-12     00:     IDLE1 selected for low-power mode
 bit 11-9      000:    PLL x4 mode
 bit 8         1:      reserved
 bit 7         0:      0 = disable ADC module clock
 bit 6         0:      0 = disable SCI module clock
 bit 5         1:      1 = enable SPI module clock
 bit 4         0:      0 = disable CAN module clock
 bit 3         1:      1 = enable EVB module clock
 bit 2         1:      1 = enable EVA module clock
 bit 1         0:      reserved
 bit 0         1:      clear the ILLADR bit
*/

//    *SCSR2 = (*SCSR2 | 0x000B) & 0x000F;
/*
 bit 15-6      0's:    reserved
 bit 5         0:      do NOT clear the WD OVERRIDE bit
 bit 4         0:      XMIF_HI-Z, 0=normal mode, 1=Hi-Z'd
 bit 3         1:      disable the boot ROM, enable the FLASH
 bit 2     no change   MP/MC* bit reflects state of MP/MC* pin
 bit 1-0      11:      11 = SARAM mapped to prog and data
*/


/* Disable the watchdog timer */
    *WDCR  = 0x00E8;
/*
 bits 15-8     0's:     reserved
 bit 7         1:       clear WD flag
 bit 6         1:       disable the dog
 bit 5-3       101:     must be written as 101
 bit 2-0       000:     WDCLK divider = 1
*/


/* Setup external memory interface for LF2407 EVM */
    WSGR = 0X0E8;
/*
 bit 15-11     0's:    reserved
 bit 10-9      00:     bus visibility off
 bit 8-6       001:    1 wait-state for I/O space
 bit 5-3       000:    0 wait-state for data space
 bit 2-0       000:    0 wait state for program space
*/
/*** Setup the event manager interrupts ***/
 	*IMR = 0x0000;				   		//Initialize IMR
	*IFR = 0x0FFFF;						//清除所有中断标志,"写1清0"

}

/*---------------------------------------------------------------------------
Name: 			IOInit()
Description:	Io port initialization, IOPE1~IOPE5 as output for SPI Control
Input par:		none
Output par:		none
----------------------------------------------------------------------------*/
int inline IOInit()
{
	//IOPE1~IOPE5 configed as general I/O, Output, default value: 0
	//Used as spi control
	*MCRC &= 0xFFC1;
	*PEDATDIR |= 0x3E00;
	*PEDATDIR &= 0xFFC1;

	//IOPB8, IOPF6 configed as general I/O, Output, default value: 1
	//Used as MOTOR C, D Serve On Signal;
	*MCRA &= 0x7FFF;
	*PBDATDIR |= 0x8080;

	*MCRC &= 0xBFFF;
	*PFDATDIR |= 0x4040;
}

/*---------------------------------------------------------------------------
Name: 			spiInit()
Description:	SPI initialization
Input par:		none
Output par:		none
----------------------------------------------------------------------------*/
void inline spiInit()
{
	*SPICCR = 0x000F;					//16 char bits,在SPICLK的下降沿发送数据
										//在上升沿输入数据****bit6必须为0
	*SPICTL = 0x000E;					//使能主动模式，一般的时钟方式，使能TALK
	*SPIBRR = 0x0013;					//系统频率40MHZ，SPI波特率2MHZ；
	*MCRB = 0x003C;						//设置通用引脚为SPI功能的引脚
	*SPICCR = *SPICCR | 0x0080;			//使SPI退出复位状态
}

/*---------------------------------------------------------------------------
Name: 			spiTransForMotor()
Description:	SPI transformation for PCM56U, to control motor
Input par:		none
Output par:		none
----------------------------------------------------------------------------*/
void spiTransForMotor()
{
	//PCM56U DLE LOW
	*PEDATDIR &= (~DA_DLE_EN);
	
	*SPITXBUF = spiValToTrans;
	while(!(*SPISTS & 0x40));
	*SPIRXBUF;							//虚读SPIRXBUF寄存器，以清除SPI中断标志
	
	//PCM56U DLE HIGH
	*PEDATDIR |= DA_DLE_EN;
}

void interrupt nothing()
{ 
	enable();
	return;
}
							
int main()
{
	int i = 0;
	disable(); 
	DspInit();
	IOInit();
	spiInit();
	
	//Initializing PCM56U output
	motorServe(MOTORA_ID, DA_ZERO_OFFSETA);
	motorServe(MOTORB_ID, DA_ZERO_OFFSETB);
	motorServe(MOTORC_ID, DA_ZERO_OFFSETC);
	motorServe(MOTORD_ID, DA_ZERO_OFFSETD);
	
	Timer3int();
	Timer1int();
	Qepinit();
	ls7266Init();

	
	c950_init();
		
	ls7266_wrXCNT(0x7FFFFF);
	ls7266_wrYCNT(0x7FFFFF);
	//initialize QepCNT value;
	motorAPos = *T2CNT - QEPA_BASE_VALUE;
	motorBPos = *T4CNT - QEPB_BASE_VALUE;
	motorCPos = ls7266_SoftRdX() - QEPC_BASE_VALUE;
	motorDPos = ls7266_SoftRdY() - QEPD_BASE_VALUE;
	
	//initialize PID parameter
	motorA_PID_Init();
	motorB_PID_Init();
	motorC_PID_Init();
	motorD_PID_Init();
	
	//PID Calculate to initialize DA value
	QepACap();
	QepBCap();
	QepCCap();
	QepDCap();
	PID_Calculate(MOTORA_ID);
	PID_Calculate(MOTORB_ID);
	PID_Calculate(MOTORC_ID);
	PID_Calculate(MOTORD_ID);
	enable();
	
	for(i = 0; i < 16; i++)
	{
		speedABuf[i] = 0;
		speedBBuf[i] = 0;
		speedCBuf[i] = 0;
		speedDBuf[i] = 0;
	}
	
	*T1CON |= 0x0040;
	
	motorServeOn(MOTORA_ID);
	motorServeOn(MOTORB_ID);
	motorServeOn(MOTORC_ID);
	motorServeOn(MOTORD_ID);	
	
	
 	while(1)
	{
		if(TimerPIntFlag == 1)
		{
			enable();                       //Enable global interrupt
			TimerPIntFlag = 0;
		}
	}		
}