📄 svmvf.c
字号:
#include "register.h"
#include "float.h"
#include "math.h"
extern int para_init();
extern int calcvel();
extern int Vel_Mech;
void qep_init();
void initial();
void initial2();
void inline enable() { asm(" clrc INTM");}
void interrupt adc_int();
unsigned int adc_res[8]={0,0,0,0,0,0,0,0};
unsigned int index=0;
int sector,cmp1,cmp2,cmp3,cmp4,cmp5,cmp6;
int triangle;
float kp;
float VHZ;
float volts;
float theta=0;
float ta,tb,tc;
float delta_theta;
/*sin(theta)*/
float sin_tab[256]={0,0.0245,0.0491,0.0736,0.0980,0.1224,0.1467,0.1710,0.1951,0.2191,0.2430,0.2667,0.2903,0.3137,0.3369,0.3599,0.3827,
0.4052,0.4276,0.4496,0.4714,0.4929,0.5141,0.5350,0.5556,0.5758,0.5957,0.6152,0.6344,0.6532,0.6716,0.6895,0.7071,0.7242,
0.7410,0.7572,0.7730,0.7883,0.8032,0.8176,0.8315,0.8449,0.8577,0.8701,0.8819,0.8932,0.9040,0.9142,0.9239,0.9330,0.9415,
0.9495,0.9569,0.9638,0.9700,0.9757,0.9808,0.9853,0.9892,0.9925,0.9952,0.9973,0.9988,0.9997,1.0000,0.9997,
0.9988,0.9973,0.9952,0.9925,0.9892,0.9853,0.9808,0.9757,0.9700,0.9638,0.9569,0.9495,0.9415,0.9330,0.9239,
0.9142,0.9040,0.8932,0.8819,0.8701,0.8577,0.8449,0.8315,0.8176,0.8032,0.7883,0.7730,0.7572,0.7410,0.7242,
0.7071,0.6895,0.6716,0.6532,0.6344,0.6152,0.5957,0.5758,0.5556,0.5350,0.5141,0.4929,0.4714,0.4496,0.4276,
0.4052,0.3827,0.3599,0.3369,0.3137,0.2903,0.2667,0.2430,0.2191,0.1951,0.1710,0.1467,0.1224,0.0980,0.0736,
0.0491,0.0245,0.0000,-0.0245,-0.0491,-0.0736,-0.0980,-0.1224,-0.1467,-0.1710,-0.1951,-0.2191,-0.2430,-0.2667,-0.2903,
-0.3137,-0.3369,-0.3599,-0.3827,-0.4052,-0.4276,-0.4496,-0.4714,-0.4929,-0.5141,-0.5350,-0.5556,-0.5758,-0.5957,-0.6152,
-0.6344,-0.6532,-0.6716,-0.6895,-0.7071,-0.7242,-0.7410,-0.7572,-0.7730,-0.7883,-0.8032,-0.8176,-0.8315,-0.8449,-0.8577,
-0.8701,-0.8819,-0.8932,-0.9040,-0.9142,-0.9239,-0.9330,-0.9415,-0.9495,-0.9569,-0.9638,-0.9700,-0.9757,-0.9808,-0.9853,
-0.9892,-0.9925,-0.9952,-0.9973,-0.9988,-0.9997,-1.0000,-0.9997,-0.9988,-0.9973,-0.9952,-0.9925,-0.9892,-0.9853,-0.9808,
-0.9757,-0.9700,-0.9638,-0.9569,-0.9495,-0.9415,-0.9330,-0.9239,-0.9142,-0.9040,-0.8932,-0.8819,-0.8701,-0.8577,-0.8449,
-0.8315,-0.8176,-0.8032,-0.7883,-0.7730,-0.7572,-0.7410,-0.7242,-0.7071,-0.6895,-0.6716,-0.6532,-0.6344,-0.6152,-0.5957,
-0.5758,-0.5556,-0.5350,-0.5141,-0.4929,-0.4714,-0.4496,-0.4276,-0.4052,-0.3827,-0.3599,-0.3369,-0.3137,-0.2903,-0.2667,
-0.2430,-0.2191,-0.1951,-0.1710,-0.1467,-0.1224,-0.0980,-0.0736,-0.0491,-0.0245};
/*sin(theta+pi/3)*/
float sin_tabp[256]={0.8660,0.8780,0.8895,0.9005,0.9109,0.9207,0.9300,0.9388,0.9469,0.9545,0.9616,0.9680,0.9739,0.9792,
0.9838,0.9879,0.9914,0.9943,0.9967,0.9984,0.9995,1.0000,0.9999,0.9992,0.9979,0.9960,0.9934,0.9903,0.9866,
0.9823,0.9775,0.9720,0.9659,0.9593,0.9521,0.9443,0.9359,0.9270,0.9175,0.9075,0.8969,0.8857,0.8741,0.8619,
0.8492,0.8360,0.8223,0.8081,0.7934,0.7782,0.7625,0.7464,0.7299,0.7129,0.6954,0.6776,0.6593,0.6407,0.6217,
0.6023,0.5825,0.5624,0.5419,0.5211,0.5000,0.4786,0.4569,0.4349,0.4127,0.3902,0.3675,0.3446,0.3214,0.2981,
0.2746,0.2509,0.2271,0.2031,0.1790,0.1548,0.1305,0.1062,0.0817,0.0572,0.0327,0.0082,-0.0164,-0.0409,-0.0654,-0.0899,-0.1143,
-0.1386,-0.1629,-0.1871,-0.2111,-0.2350,-0.2588,-0.2824,-0.3059,-0.3292,-0.3523,-0.3751,-0.3977,-0.4201,-0.4423,-0.4642,
-0.4858,-0.5071,-0.5281,-0.5487,-0.5691,-0.5891,-0.6088,-0.6280,-0.6470,-0.6655,-0.6836,-0.7013,-0.7186,-0.7354,-0.7518,
-0.7678,-0.7833,-0.7983,-0.8128,-0.8269,-0.8404,-0.8535,-0.8660,-0.8780,-0.8895,-0.9005,-0.9109,-0.9207,-0.9300,-0.9388,
-0.9469,-0.9545,-0.9616,-0.9680,-0.9739,-0.9792,-0.9838,-0.9879,-0.9914,-0.9943,-0.9967,-0.9984,-0.9995,-1.0000,-0.9999,
-0.9992,-0.9979,-0.9960,-0.9934,-0.9903,-0.9866,-0.9823,-0.9775,-0.9720,-0.9659,-0.9593,-0.9521,-0.9443,-0.9359,-0.9270,
-0.9175,-0.9075,-0.8969,-0.8857,-0.8741,-0.8619,-0.8492,-0.8360,-0.8223,-0.8081,-0.7934,-0.7782,-0.7625,-0.7464,-0.7299,
-0.7129,-0.6954,-0.6776,-0.6593,-0.6407,-0.6217,-0.6023,-0.5825,-0.5624,-0.5419,-0.5211,-0.5000,-0.4786,-0.4569,-0.4349,
-0.4127,-0.3902,-0.3675,-0.3446,-0.3214,-0.2981,-0.2746,-0.2509,-0.2271,-0.2031,-0.1790,-0.1548,-0.1305,-0.1062,-0.0817,
-0.0572,-0.0327,-0.0082,0.0164,0.0409,0.0654,0.0899,0.1143,0.1386,0.1629,0.1871,0.2111,0.2350,
0.2588,0.2824,0.3059,0.3292,0.3523,0.3751,0.3977,0.4201,0.4423,0.4642,0.4858,0.5071,0.5281,0.5487,0.5691,0.5891,0.6088,
0.6280,0.6470,0.6655,0.6836,0.7013,0.7186,0.7354,0.7518,0.7678,0.7833,0.7983,0.8128,0.8269,0.8404,0.8535};
/*sin(theta-pi/3)*/
float sin_tabn[256]={-0.8660,-0.8535,-0.8404,-0.8269,-0.8128,-0.7983,-0.7833,-0.7678,-0.7518,-0.7354,-0.7186,-0.7013,-0.6836,-0.6655,-0.6470,
-0.6280,-0.6088,-0.5891,-0.5691,-0.5487,-0.5281,-0.5071,-0.4858,-0.4642,-0.4423,-0.4201,-0.3977,-0.3751,-0.3523,-0.3292,
-0.3059,-0.2824,-0.2588,-0.2350,-0.2111,-0.1871,-0.1629,-0.1386,-0.1143,-0.0899,-0.0654,-0.0409,-0.0164,0.0082,0.0327,
0.0572,0.0817,0.1062,0.1305,0.1548,0.1790,0.2031,0.2271,0.2509,0.2746,0.2981,0.3214,0.3446,0.3675,0.3902,0.4127,0.4349,
0.4569,0.4786,0.5000,0.5211,0.5419,0.5624,0.5825,0.6023,0.6217,0.6407,0.6593,0.6776,0.6954,0.7129,0.7299,0.7464,0.7625,
0.7782,0.7934,0.8081,0.8223,0.8360,0.8492,0.8619,0.8741,0.8857,0.8969,0.9075,0.9175,0.9270,0.9359,0.9443,
0.9521,0.9593,0.9659,0.9720,0.9775,0.9823,0.9866,0.9903,0.9934,0.9960,0.9979,0.9992,0.9999,1.0000,0.9995,
0.9984,0.9967,0.9943,0.9914,0.9879,0.9838,0.9792,0.9739,0.9680,0.9616,0.9545,0.9469,0.9388,0.9300,0.9207,
0.9109,0.9005,0.8895,0.8780,0.8660,0.8535,0.8404,0.8269,0.8128,0.7983,0.7833,0.7678,0.7518,0.7354,0.7186,
0.7013,0.6836,0.6655,0.6470,0.6280,0.6088,0.5891,0.5691,0.5487,0.5281,0.5071,0.4858,0.4642,0.4423,0.4201,
0.3977,0.3751,0.3523,0.3292,0.3059,0.2824,0.2588,0.2350,0.2111,0.1871,0.1629,0.1386,0.1143,0.0899,0.0654,
0.0409,0.0164,-0.0082,-0.0327,-0.0572,-0.0817,-0.1062,-0.1305,-0.1548,-0.1790,-0.2031,-0.2271,-0.2509,-0.2746,-0.2981,
-0.3214,-0.3446,-0.3675,-0.3902,-0.4127,-0.4349,-0.4569,-0.4786,-0.5000,-0.5211,-0.5419,-0.5624,-0.5825,-0.6023,-0.6217,
-0.6407,-0.6593,-0.6776,-0.6954,-0.7129,-0.7299,-0.7464,-0.7625,-0.7782,-0.7934,-0.8081,-0.8223,-0.8360,-0.8492,-0.8619,
-0.8741,-0.8857,-0.8969,-0.9075,-0.9175,-0.9270,-0.9359,-0.9443,-0.9521,-0.9593,-0.9659,-0.9720,-0.9775,-0.9823,-0.9866,
-0.9903,-0.9934,-0.9960,-0.9979,-0.9992,-0.9999,-1.0000,-0.9995,-0.9984,-0.9967,-0.9943,-0.9914,-0.9879,-0.9838,-0.9792,
-0.9739,-0.9680,-0.9616,-0.9545,-0.9469,-0.9388,-0.9300,-0.9207,-0.9109,-0.9005,-0.8895,-0.8780};
#define pi 3.1415926
#define pi2 2*pi
#define pi3 pi/3
#define pi6 pi/6
#define ts 0x2328
#define ts2 0x1194
#define vdcalfa 180 /*230 vdcalfa=Vdc/3*/
#define vdcbeta 156 /*200 vdcbeta=sqrt(3)*Vdc/6*/
void interrupt nothing(void)
{
enable();
return;
}
void inline disable()
{
asm(" setc INTM");
return;
}
void initial()
{
asm(" setc SXM");
asm(" setc OVM");
asm(" clrc CNF");
*SCSR1=0x838E;
*WDCR=0x0E8;
*IMR=0x0;
*IFR=0x0FFFF;
return;
}
void qep_init(void)
{
*T2PER=0x0FFFF;
*CAPCONA=0x0E000;
*T2CNT=0x00;
*T2CON=0x1870;
}
void initial2()
{
*IMR=0x0003;
*T1PER=0x1194;
*T3PER=0x1194;
*CAPCONA=0x0E000;
*GPTCONA=0x0041;
*GPTCONB=0x044A;
*EVAIMRA=0x0201;
*EVBIMRA=0x0001;
*T1CON=0x0842;
*T3CON=0x0842;
*COMCONA=0x8200;
*COMCONB=0x8200;
*T1CNT=0x00;
*T3CNT=0x00;
*ACTRA=0x0999;
*ACTRB=0x0999;
*CMPR1=0x1194;
*CMPR2=0x1194;
*CMPR3=0x1194;
*CMPR4=0x1194;
*CMPR5=0x1194;
*CMPR6=0x1194;
*DBTCONA=0x0cf0;
*DBTCONB=0x0cf0;
*EVAIFRA=0x0FFFF;
*EVBIFRA=0x0FFFF;
*EVAIFRB=0x0004;
*EVAIMRB=0x0004;
*MCRA=*MCRA|0x0FD8;
*MCRC=*MCRC|0x0C7E;
WSGR=0x00;
return;
}
/**********************************************************/
void init_adc(void)
{
*T4CNT=0x0000;
*T4CON=0x0842;
*T4PER=0x2000;
*GPTCONB=0x44A;
*EVBIFRB=0xffff;
*ADCTRL1=0x0010;
*ADCTRL2=0x8404;
*MAXCONV=0x07;
*CHSELSEQ1=0x3210;
*CHSELSEQ2=0x7654;
}
void interrupt adc_int(void)
{
unsigned int i;
extern int Vel_Mech;
switch(* PVIR)
{
case 0x0004:
{
asm(" clrc SXM");
calcvel();
*T4CMP=Vel_Mech<<1;
/*spd_new=*T2CNT;
incre_spd=spd_new-spd_old;
spd_old=spd_new;*/
for(i=0;i<8;i++)
adc_res[i]=*(RESULT0+i)>>6;
/*if ((adc_res[4])>=800){
adc_res[4]=4080;
}*/
/**T4CMP=2*Vel_Mech;/*adc_res[4];*/
*T3CMP=4*adc_res[4];/*Vel_Mech;*/
VHZ=(adc_res[4])*0.05;
delta_theta=0.001885*VHZ;
if (VHZ>=50){
volts=310;
}
else if(VHZ<=5){
volts=10;
}
else {
volts=6.5*VHZ-22;
}
kp=(1.73205*volts)/540;
break;
}
default:
;
}
*ADCTRL2|=0x4200; /*reset SEQ1,and clear INT FLAG SEQ1*/
/**IFR|=0x0003; /*enable INT1 interrupt*/
asm(" CLRC INTM"); /*enable interrupt*/
}
/***************start ad convertion*******************************/
void ADSOC()
{
*T4CON=*T4CON|0X0040;
return;
}
/*****************************************************************/
void interrupt PWM_ISR()
{
*EVAIFRA=*EVAIFRA|0x0200;
/*theta=i*pi2/n; /*n=60*/
if((theta>=0)&(theta<pi3))
{
sector=1;
index=(unsigned int)(theta*40.744);
if(volts*sin_tabp[index]<=vdcbeta)/*sin(theta+pi/3)*/
{ triangle=1;
ta=(-ts)*kp*sin_tabn[index];/*(pi3-theta);*/
tb=(ts2)*(1-2*kp*sin_tabp[index]);/*(theta+pi3));*/
tc=kp*ts*sin_tab[index];/*(theta);*/
if(theta<=pi6)
{
cmp1=(unsigned int)(ta)>>1;
cmp2=(unsigned int)(ts2);
cmp3=0;
cmp4=(unsigned int)(ta/2+tb+tc);
cmp5=0;
cmp6=(unsigned int)(ta/2+tb);
}
else /*A12*/
{
cmp1=(unsigned int)(tc/2+ta);
cmp2=(unsigned int)(ts2);
cmp3=(unsigned int)(tc)>>1;
cmp4=(unsigned int)(ts2);
cmp5=0;
cmp6=(unsigned int)(ta+tb+tc/2);
}
}
else if((-volts)*sin_tabn[index]>vdcbeta)/*sin(pi3-theta) */
{
triangle=2;
ta=ts*(1-kp*sin_tabp[index]);/*(theta+pi3)); */
tb=kp*ts*sin_tab[index];
tc=(ts2)*(2*(-kp)*sin_tabn[index]-1);
cmp1=(unsigned int)(ta/2+tb+tc);
cmp2=(unsigned int)(ts2);
cmp3=0;
cmp4=(unsigned int)(ta/2+tb);
cmp5=0;
cmp6=(unsigned int)(ta)>>1;
}
else if(volts*sin_tab[index]>vdcbeta)
{
triangle=4;
ta=(ts2)*(2*kp*sin_tab[index]-1);
tb=(-kp)*ts*sin_tabn[index];
tc=ts*(1-kp*sin_tabp[index]);
cmp1=(unsigned int)(tc/2+ta+tb);
cmp2=(unsigned int)(ts2);
cmp3=(unsigned int)(tc/2+ta);
cmp4=(unsigned int)(ts2);
cmp5=0;
cmp6=(unsigned int)(tc)>>1;
}
else
{
triangle=3;
ta=(ts2)*(1-2*kp*sin_tab[index]);
tb=(ts2)*(2*kp*sin_tabp[index]-1);
tc=(ts2)*(1+2*kp*sin_tabn[index]);
if (theta<pi6)
{
cmp1=(unsigned int)(ta/2+tb);
cmp2=(unsigned int)(ts2);
cmp3=0;
cmp4=(unsigned int)(ta/2+tb+tc);
cmp5=0;
cmp6=(unsigned int)(ta)>>1;
}
else
{
cmp1=(unsigned int)(ta+tb+tc/2);
cmp2=(unsigned int)(ts2);
cmp3=(unsigned int)(tc)>>1;
cmp4=(unsigned int)(ts2);
cmp5=0;
cmp6=(unsigned int)(ta+tc/2);
}
}
}
/******************************************************************/
/*2 sector*/
/******************************************************************/
else if((theta>=pi3)&&(theta<2*pi3))
{
sector=2;
index=(unsigned int)(theta*40.744);
if(volts*sin_tab[index]<=vdcbeta)
{triangle=1;
ta=kp*ts*sin_tabp[index];
tb=(ts2)*(1-2*kp*sin_tab[index]);
tc=kp*ts*sin_tabn[index];
if (theta<3*pi6)
{
cmp1=(unsigned int)(ta)>>1;
cmp2=(unsigned int)(ts2);
cmp3=(unsigned int)(ta/2+tc);
cmp4=(unsigned int)(ts2);
cmp5=0;
cmp6=(unsigned int)(ta/2+tb+tc);
}
else
{
cmp1=0;
cmp2=(unsigned int)(ta+tb+tc/2);
cmp3=(unsigned int)(tc)>>1;
cmp4=(unsigned int)(ts2);
cmp5=0;
cmp6=(unsigned int)(tb+tc/2);
}
}
else if(volts*sin_tabp[index]>vdcbeta)
{
triangle=2;
ta=ts*(1-kp*sin_tab[index]);
tb=kp*ts*sin_tabn[index];
tc=(ts2)*(2*kp*sin_tabp[index]-1);
cmp1=(unsigned int)(ta/2+tc);
cmp2=(unsigned int)(ts2);
cmp3=(unsigned int)(ta/2+tb+tc);
cmp4=(unsigned int)(ts2);
cmp5=0;
cmp6=(unsigned int)(ta)>>1;
}
else if(volts*sin_tabn[index]>vdcbeta)
{
triangle=4;
ta=(ts2)*(2*kp*sin_tabn[index]-1);
tb=kp*ts*sin_tabp[index];
tc=ts*(1-kp*sin_tab[index]);
cmp1=0;
cmp2=(unsigned int)(tc/2+tb);
cmp3=(unsigned int)(tc/2+ta+tb);
cmp4=(unsigned int)(ts2);
cmp5=0;
cmp6=(unsigned int)(tc)>>1;
}
else
{
triangle=3;
ta=(ts2)*(1-2*kp*sin_tabn[index]);
tb=(ts2)*(2*kp*sin_tab[index]-1);
tc=(ts2)*(1-2*kp*sin_tabp[index]);
if (theta<3*pi6)
{
cmp1=(unsigned int)(ta)>>1;
cmp2=(unsigned int)(ts2);
cmp3=(unsigned int)(ta/2+tb+tc);
cmp4=(unsigned int)(ts2);
cmp5=0;
cmp6=(unsigned int)(ta/2+tc);
}
else
{
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -