cas_inv.c

DSP TMS320LF2407A 转速闭环控制系统

#include "2407A.h"
#include "cpldport.h"

#define Uint unsigned int
#define Ulong unsigned long

#define K_MN 44739//U/f=C，即M/n=C，这里设M/n=0.000666，Q26格式
#define K_NE 49152//转速和光编脉冲增量之间的转换系数,0.75,Q16格式
#define TSM 37500//37500/(30M/8)=0.01
#define EP 8000//光编脉冲计数器的周期，即光编旋转一周的计数值
#define NMAX 3000//最大转速
#define NMIN 10//最小转速
#define NSTEP 240//步进转速,Q3格式

#define RUN *PADATDIR&=0xffbf//开启脉冲发生器
#define STOP *PADATDIR|=0x0040//封锁脉冲发生器
#define ESET *PADATDIR&=0xff7f//使能载波发生器峰值寄存器设置
#define DSET *PADATDIR|=0x0080//禁止载波发生器峰值寄存器设置
#define ESETP *PBDATDIR&=0xfffe//使能载波发生器初值设置
#define DSETP *PBDATDIR|=0x0001//禁止载波发生器初值设置

asm(" .ref _c_int0");
/*---------------------------------------正弦表----------------------------------------*/
const int sintab[513]=//将第一象限分为512度，Q15格式
{
	0,101,201,302,402,503,603,704,804,905,1005,1106,1206,1307,1407,1507,1608,1708,
	1809,1909,2009,2110,2210,2310,2411,2511,2611,2711,2811,2912,3012,3112,3212,3312,
	3412,3512,3612,3712,3812,3911,4011,4111,4211,4310,4410,4510,4609,4709,4808,4907,
	5007,5106,5205,5305,5404,5503,5602,5701,5800,5899,5998,6097,6195,6294,6393,6491,
	6590,6688,6787,6885,6983,7081,7180,7278,7376,7473,7571,7669,7767,7864,7962,8059,
	8157,8254,8351,8449,8546,8643,8740,8836,8933,9030,9127,9223,9319,9416,9512,9608,
	9704,9800,9896,9992,10088,10183,10279,10374,10469,10565,10660,10755,10850,10945,
	11039,11134,11228,11323,11417,11511,11605,11699,11793,11887,11980,12074,12167,
	12261,12354,12447,12540,12633,12725,12818,12910,13003,13095,13187,13279,13371,
	13463,13554,13646,13737,13828,13919,14010,14101,14192,14282,14373,14463,14553,
	14643,14733,14823,14912,15002,15091,15180,15269,15358,15447,15535,15624,15712,
	15800,15888,15976,16064,16151,16239,16326,16413,16500,16587,16673,16760,16846,
	16932,17018,17104,17190,17275,17361,17446,17531,17616,17700,17785,17869,17953,
	18037,18121,18205,18288,18372,18455,18538,18621,18703,18786,18868,18950,19032,
	19114,19195,19277,19358,19439,19520,19601,19681,19761,19841,19921,20001,20081,
	20160,20239,20318,20397,20475,20554,20632,20710,20788,20865,20943,21020,21097,
	21174,21251,21327,21403,21479,21555,21631,21706,21781,21856,21931,22006,22080,
	22154,22228,22302,22375,22449,22522,22595,22668,22740,22812,22884,22956,23028,
	23099,23170,23241,23312,23383,23453,23523,23593,23663,23732,23801,23870,23939,
	24008,24076,24144,24212,24279,24347,24414,24481,24548,24614,24680,24746,24812,
	24878,24943,25008,25073,25138,25202,25266,25330,25394,25457,25520,25583,25646,
	25708,25771,25833,25894,25956,26017,26078,26139,26199,26259,26320,26379,26439,
	26498,26557,26616,26674,26733,26791,26848,26906,26963,27020,27077,27133,27190,
	27246,27301,27357,27412,27467,27522,27576,27630,27684,27738,27791,27844,27897,
	27950,28002,28054,28106,28158,28209,28260,28311,28361,28411,28461,28511,28560,
	28610,28658,28707,28755,28803,28851,28899,28946,28993,29040,29086,29132,29178,
	29224,29269,29314,29359,29404,29448,29492,29535,29579,29622,29665,29707,29750,
	29792,29833,29875,29916,29957,29997,30038,30078,30118,30157,30196,30235,30274,
	30312,30350,30388,30425,30462,30499,30536,30572,30608,30644,30680,30715,30750,
	30784,30819,30853,30886,30920,30953,30986,31018,31050,31082,31114,31146,31177,
	31207,31238,31268,31298,31328,31357,31386,31415,31443,31471,31499,31527,31554,
	31581,31608,31634,31660,31686,31711,31737,31761,31786,31810,31834,31858,31881,
	31904,31927,31950,31972,31994,32015,32037,32058,32078,32099,32119,32138,32158,
	32177,32196,32214,32233,32251,32268,32286,32303,32319,32336,32352,32368,32383,
	32398,32413,32428,32442,32456,32470,32483,32496,32509,32522,32534,32546,32557,
	32568,32579,32590,32600,32610,32620,32629,32638,32647,32656,32664,32672,32679,
	32686,32693,32700,32706,32712,32718,32723,32729,32733,32738,32742,32746,32749,
	32753,32756,32758,32760,32762,32764,32766,32767,32767,32767,32767
};
/*--------------------------------------载波比表---------------------------------------*/
const Uint modulate_N[24]=//以128r/min为一段所对应的载波比
{
	585,291,189,135,105,87,75,63,57,51,45,39,39,33,33,33,27,27,27,27,21,21,21,21
};
/*------------------------------------全局变量定义-------------------------------------*/
Uint flag_run=0,flag_sendn=0,flag_send='n';//电机是否正在运转标志,发送数据标志
Uint Toff1_al,Toff1_ar,Toff1_bl,Toff1_br,Toff1_cl,Toff1_cr;//各相各桥臂Toff1对应的计数值
Uint modulate_M=32767;//调制比，取1，Q15格式
Uint Tc=5130;//载波周期,由于CPLD计数频率为10MHz,所以单位为0.1us,10000000/50/39=5130
Uint phase=0,step=560;//初始相位,初始步长：2048/39/3，Q5格式
Uint cnt_rx=0,cnt_tx=0,cnt_t1p=0,cnt_sendn=0;//接收,发送字节次数计数器
int cnt_ep=0;//光编脉冲周期计数器
char command;//上位机发来的指令
int n_appoint=12000,n_anow=0,n_actual=0;//上位机发来的转速指令,现时的给定转速,实际转速,单位r/min,Q3格式
int temp_n;//接收新转速时的暂存单元
Uint temp_sendn=1000;
int encoder_new=0,encoder_old=0,encoder_inc=0;//光编脉冲计数器新值,旧值,增量
/*-------------------------------------禁止看门狗--------------------------------------*/
// This function resets the watchdog timer.
void KickDog(void)
{
    *WDCR|=0x68;
}
/*-------------------------------------延时子程序--------------------------------------*/
void delay(Uint t)
{
	while(t--);
}
/*------------------------------------计算单相Toff1------------------------------------*/
void cal_Toff1_s(Uint pha,Uint *tl,Uint *tr)
{
	Uint k=pha>>5;//转化为Q0格式
	int temp;
	switch(k>>9)
	{//temp=M*sin(pha)
		case 0:temp=((long)modulate_M*(long)sintab[k])>>15;break;
		case 1:temp=((long)modulate_M*(long)sintab[1024-k])>>15;break;
		case 2:temp=((long)modulate_M*(long)(-sintab[k-1024]))>>15;break;
		case 3:temp=((long)modulate_M*(long)(-sintab[2048-k]))>>15;break;
	}
	*tl=(Ulong)(32768-temp)*(Ulong)Tc>>17;//Toff1_xl=(Tc*(1-temp)/4)
	*tr=(Ulong)(32768+temp)*(Ulong)Tc>>17;//Toff1_xr=(Tc*(1+temp)/4)
}
/*--------------------------------------计算Toff1--------------------------------------*/
void cal_Toff1(void)
{
	Uint Toff1_sum=3*Tc>>2;
	cal_Toff1_s(phase,&Toff1_al,&Toff1_ar);//计算Toff1_al,Toff1_ar
	if(n_anow>0)cal_Toff1_s(phase-21845,&Toff1_bl,&Toff1_br);
	else cal_Toff1_s(phase+21845,&Toff1_bl,&Toff1_br);
	//计算Toff1_bl,Toff1_br,正转则B相落后A相120度，反转则B相超前A相120度
	Toff1_cl=Toff1_sum-Toff1_al-Toff1_bl;//3*Tc/4-Toff1_al-Toff1_bl
	Toff1_cr=Toff1_sum-Toff1_ar-Toff1_br;//3*Tc/4-Toff1_ar-Toff1_br
}
/*---------------------------------------调整转速--------------------------------------*/
void n_modulation(void)
{
	Uint n,k,temp;
	int e=n_appoint-n_anow;
	if(e<-NSTEP)n_anow-=NSTEP;
	else if(e>NSTEP)n_anow+=NSTEP;
	else n_anow=n_appoint;
	n=abs(n_anow);
	k=n>>10;
	STOP;
	if(n!=0)
	{
		modulate_M=n>=12000?32767:(Ulong)K_MN*(Ulong)n>>14;
		//由于U/f要保持恒定，所以变频调速时M也要变，结果为Q15格式
		temp=(Ulong)2400000000/(Ulong)(n*modulate_N[k]);
		temp=(Uint)((Ulong)(temp+3)*43691>>18)*2;//temp=Tc/3
		Tc=temp*3;
		step=(Ulong)65536/(3*modulate_N[k]);
		
		ESET;//允许载波发生器峰值寄存器设置
		delay(0);//延时，确保峰值寄存器设置使能成功
		CW1_TC=0x8009;//给载波计数器赋初值，使其相差120度，9|0x8000
		CW2_TC=(9+temp)|0x8000;
		CW3_TC=temp-9;
		ESETP;//使能载波发生器初值设置
		delay(0);//延时，确保初值设置成功
		DSETP;
		temp=Tc>>1;//令temp=Tc/2，同时起延时作用
		
		CW1_TC=temp;//发送载波峰值
		CW2_TC=temp;
		CW3_TC=temp;
		
		DSET;//禁止载波发生器峰值寄存器设置
		
		cal_Toff1();//计算第一组Toff1,并发送给第1个载波单元
		CW1_TAL=Toff1_al;
		CW1_TAR=Toff1_ar;
		CW1_TBL=Toff1_bl;
		CW1_TBR=Toff1_br;
		CW1_TCL=Toff1_cl;
		CW1_TCR=Toff1_cr;
		phase+=step;
		cal_Toff1();//计算第二组Toff1,以备第一次中断来时发送给第2个载波单元
		RUN;
	}
}
/*--------------------------------------系统初始化-------------------------------------*/
// This function initializes the System Control registers to a known state.
void InitSysCtrl(void)
{
   *SCSR1=0x024d;//时钟30M,使能EVAclock,EVBclock,SCIclock
   *WDCR=0xe8;//禁止看门狗
   *WSGR=0x0000;//禁止所有的等待状态
   asm(" clrc OVM"); //采用正常溢出方式
}
/*---------------------------------------EV初始化--------------------------------------*/
void InitEv(void)
{
    *CAPCONA=0x36a8;//使能cap1,2,3，并规定检测边沿
    *EVAIMRC|=0x0007;//使能cap1,2,3中断
    *EVAIFRC|=0xffff;//复位cap1,2,3的中断标志位
    *MCRA|=0x0038;//设置IOPA3、4、5为基本功能，即CAP1／QEP1、CAP2／QEP2、CAP3
	*T1CNT=0;//设置定时器1的初始值为0
	*T1PR=TSM-1;//设置定时器1的周期为TSM
    *T1CON=0x1340;//连续增计数模式,32分频,内部时钟,使能T1
    *EVAIFRA|=0x0080;//复位T1周期中断标志位
    *EVAIMRA|=0x0080;//使能T1周期中断
    *T4CNT=0;//设置定时器4的初始值为0
    *T4PR=EP-1;//设置定时器4的周期为EP
    *T4CON=0x1870;//定向增/减计数模式,不分频,QEP时钟,使能T4
    *CAPCONB=0x0000;//清QEP
    *EVBIFRB|=0x0001;//复位T4周期中断标志位
    *EVBIMRB|=0x0001;//使能T4周期中断
	*MCRC|=0x0180;//设置IOPE7,IOPF0为基本功能,即CAP4/QEP3和CAP5/QEP4
	*IMR|=0x000e;//使能INT2,3,4
}
/*--------------------------------------SCI初始化--------------------------------------*/
void InitSci(void)
{
    *SCICCR=0x07;//1个停止位，不使能奇偶校验，空闲线多处理器模式，8位字符 
    *SCICTL1=0x03;//RESET SCI，使能发送和接收
    *SCICTL2=0x03;//使能发送和接收中断
    *SCIHBAUD=0x01;
	*SCILBAUD=0x86;//波特率9600b/s
	*SCICTL1=0x23 ;//使能SCI，使其脱离复位状态，使能发送和接收
    *SCIPRI=0x60;//SCI低优先级中断
    *MCRA|=0x0003;//设置IOPA0和IOPA1为基本功能，即SCITXD和SCIRXD
    *IMR|=0x0010;//开INT5
}
/*---------------------------------------初始化----------------------------------------*/
void Init(void)
{
	InitSysCtrl();//系统时钟等初始化
	
	*PADATDIR|=0xc000;//将IOPA6、IOPA7配置为输出方式
	*PBDATDIR|=0x0100;//将IOPB0配置为输出方式
	STOP;//封锁脉冲发生器
	DSET;//禁止载波发生器峰值寄存器设置
	DSETP;//禁止载波发生器初值设置
	
	DINT;//关全局中断
    *IMR=0x0000;//禁止所有中断
    	
    InitEv();//事件管理器初始化
    InitSci();//SCI初始化
    
    *IFR|=0xffff;//清除所有中断标志
    EINT;//开全局中断
}
/*--------------------------------CAP1INT中断服务子程序-------------------------------*/
void cap1int_isr(void)