📄 fuzzzy.c
字号:
#include "register.h"
#include "float.h"
#include "math.h"
unsigned int adc_res[8]={0,0,0,0,0,0,0,0};
int resultA,resultB,resultC;
int voltage1,voltage2;
int sector,triangle;
int delta_err_voltage,err_voltage,old_err_voltage;
float kp;
float volts;
float VHZ;
float m,n;
int j=60;
float ceta;
float ta,tb,tc;
unsigned int i=0;
unsigned int cmp1,cmp2,cmp3,cmp4,cmp5,cmp6;
#define pi 3.141593
#define pi2 pi/2
#define pi3 pi/3
#define pi6 pi/6
#define ts 9600
#define ts2 4800
/*#define kp 0.90 */
#define vdcalfa 180
#define vdcbeta 156
/*sin(theta)*/
float sin_tab[60]={0,0.1045,0.2079,0.3090,0.4067,0.5,0.5878,0.6691,0.7431,0.8090,
0.8660,0.9135,0.9511,0.9782,0.9945,1,0.9945,0.9782,0.9511,0.9135,
0.8660,0.8090,0.7431,0.6691,0.5878,0.5,0.4067,0.3090,0.2079,0.1045,
0,-0.1045,-0.2079,-0.3090,-0.4067,-0.5,-0.5878,-0.6691,-0.7431,-0.8090,
-0.8660,-0.9135,-0.9511,-0.9782,-0.9945,-1,-0.9945,-0.9782,-0.9511,-0.9135,
-0.8660,-0.8090,-0.7431,-0.6691,-0.5878,-0.5,-0.4067,-0.3090,-0.2079,-0.1045
};
/*sin(theta+pi/3)*/
float sin_tabp[60]={0.8660,0.9135,0.9511,0.9782,0.9945,1,0.9945,0.9782,0.9511,0.9135,
0.8660,0.8090,0.7431,0.6691,0.5878,0.5,0.4067,0.3090,0.2079,0.1045,
0,-0.1045,-0.2079,-0.3090,-0.4067,-0.5,-0.5878,-0.6691,-0.7431,-0.8090,
-0.8660,-0.9135,-0.9511,-0.9782,-0.9945,-1,-0.9945,-0.9782,-0.9511,-0.9135,
-0.8660,-0.8090,-0.7431,-0.6691,-0.5878,-0.5,-0.4067,-0.3090,-0.2079,-0.1045,
0,0.1045,0.2079,0.3090,0.4067,0.5,0.5878,0.6691,0.7431,0.8090
};
/*sin(theta-pi/3)*/
float sin_tabn[60]={-0.8660,-0.8090,-0.7431,-0.6691,-0.5878,-0.5,-0.4067,-0.3090,-0.2079,-0.1045,
0,0.1045,0.2079,0.3090,0.4067,0.5,0.5878,0.6691,0.7431,0.8090,
0.8660,0.9135,0.9511,0.9782,0.9945,1,0.9945,0.9782,0.9511,0.9135,
0.8660,0.8090,0.7431,0.6691,0.5878,0.5,0.4067,0.3090,0.2079,0.1045,
0,-0.1045,-0.2079,-0.3090,-0.4067,-0.5,-0.5878,-0.6691,-0.7431,-0.8090,
-0.8660,-0.9135,-0.9511,-0.9782,-0.9945,-1,-0.9945,-0.9782,-0.9511,-0.9135
};
/**************enable***********************/
void inline enable()
{
asm(" clrc INTM");
}
/*************PHANTOM interrupt*************/
void interrupt nothing(void)
{
enable();
return;
}
/************disable************************/
void inline disable()
{
asm(" setc INTM");
return;
}
/************dsp_initial********************/
void dsp_init()
{
asm(" setc SXM");
asm(" clrc OVM");
asm(" clrc CNF");
*SCSR1=0x838E;
*WDCR=0x0E8;
*IMR=0x0;
*IFR=0x0FFFF;
return;
}
/*******************QEP intial******************/
void qep_init(void)
{
*T2PER=0x0FFFF;
*CAPCONA=0x0E000;
*T2CNT=0x00;
*T2CON=0x1870;
}
/****************port initial******************/
void peri_init()
{
*IMR=0x0003;
*T1PER=4800;/*0x1388;*/
*T3PER=4800;/*0x1388;*/
/*CAPCONA=0x0E000; */
*GPTCONA=0x0041;
*GPTCONB=0x0440;
*EVAIMRA=0x0201;
*EVBIMRA=0x0001;
*T1CON=0x0842;
*T3CON=0x0842;
*COMCONA=0x8200;
*COMCONB=0x8200;
*T1CNT=0x00;
*T3CNT=0x00;
*ACTRA=0x0999;
*ACTRB=0x0999;
*CMPR1=4800;/*0x1388; */
*CMPR2=4800;/*0x1388; */
*CMPR3=4800;/*0x1388; */
*CMPR4=4800;/*0x1388; */
*CMPR5=4800;/*0x1388; */
*CMPR6=4800;/*0x1388; */
*DBTCONA=0x0cf0;
*DBTCONB=0x0cf0;
*EVAIFRA=0x0FFFF;
*EVBIFRA=0x0FFFF;
*EVAIFRB=0x0004;
*EVAIMRB=0x0004;
*MCRA=*MCRA|0x0FD8;
*MCRC=*MCRC|0x087E;
WSGR=0x00;
return;
}
/***************trig A/D convertion*************/
void ADSOC()
{
*T4CON=*T4CON|0X0040;
return;
}
/*************adc_interrupt intial**************/
void init_adc(void)
{
*T4CNT=0x0000;
*T4CON=0x0842;/*E4C;*/
*T4PER=10000;/*010000;*/
*GPTCONB=0x400;
*EVBIFRB=0xffff;
*ADCTRL1=0x0010;
*ADCTRL2=0x8404;
*MAXCONV=0x07;
*CHSELSEQ1=0x3210;
*CHSELSEQ2=0x7654;
}
/******************adc_interrupt****************/
void interrupt adc_int(void)
{
unsigned int i;
switch(* PVIR)
{
case 0x0004:
{
asm(" setc SXM");
for(i=0;i<8;i++)
adc_res[i]=*(RESULT0+i)>>6;
VHZ=(adc_res[4])*0.05;
if (VHZ>=50){
volts=310;
}
else if(VHZ<=5){
volts=10;
}
else {
volts=6.5*VHZ-22;
}
kp=(1.73205*volts)/540;
resultA=adc_res[1];
resultB=adc_res[0];
/*present the up capacity*/
voltage1=adc_res[5];
/*present the lower capacity*/
voltage2=2*adc_res[6];
/*error voltage*/
err_voltage=voltage1-voltage2;
/*change of error voltage*/
delta_err_voltage=err_voltage-old_err_voltage;
old_err_voltage=err_voltage;
/*err continue to rise postive*/ /*then postive segment<<negtive*/
if((err_voltage>0)&(delta_err_voltage>0))
{
m=0.2; /*postive vector mag*/
n=0.2;
}
/*err is dowm*/ /*then postive segment<negtive*/
else if ((err_voltage>0)&(delta_err_voltage<0))
{
m=0.4; /*postive vector mag*/
n=0.4;
}
/*err is rise*/ /*then postive>>negtive*/
else if ((err_voltage<0)&(delta_err_voltage<0))
{
m=0.8; /*negtive vector mag but error voltgae is in rise trend*/
n=0.8;
}
/*err is down*/ /*then postive>negtive*/
else if ((err_voltage<0)&(delta_err_voltage>0))
{
m=0.6; /*negtive vector mag*/
n=0.6;
}
else
{
m=0.5;
n=0.5;
}
break;
}
default:
;
}
*ADCTRL2|=0x4200; /*reset SEQ1,and clear INT FLAG SEQ1*/
/**IFR|=0x0003; /*enable INT1 interrupt*/
asm(" CLRC INTM"); /*enable interrupt*/
}
/********************SVM*****************************/
void interrupt PWM_ISR()
{
*EVAIFRA=*EVAIFRA|0x0200;
ceta=i*2*pi/j; /*n=60*/
if((ceta>=0)&(ceta<pi3))
{
sector=1;
if(volts*sin_tabp[i]<=vdcbeta)/*sin(ceta+pi/3)*/
{ triangle=1;
ta=(-ts)*kp*sin_tabn[i];/*(pi3-ceta);*/
tb=ts2*(1-2*kp*sin_tabp[i]);/*(ceta+pi3));*/
tc=kp*ts*sin_tab[i];/*(ceta);*/
if(ceta<=pi6)
{
cmp1=(unsigned int)(m*ta);
cmp2=(unsigned int)ts2;
cmp3=0;
cmp4=(unsigned int)(m*ta+tb+tc);
cmp5=0;
cmp6=(unsigned int)(m*ta+tb);
}
else /*A12*/
{
cmp1=(unsigned int)(n*tc+ta);
cmp2=(unsigned int)ts2;
cmp3=(unsigned int)(n*tc);
cmp4=(unsigned int)ts2;
cmp5=0;
cmp6=(unsigned int)(ta+tb+n*tc);
}
}
else if((-volts)*sin_tabn[i]>vdcbeta)/*sin(pi3-ceta) */
{
triangle=2;
ta=ts*(1-kp*sin_tabp[i]);/*(ceta+pi3)); */
tb=kp*ts*sin_tab[i];
tc=ts2*(2*(-kp)*sin_tabn[i]-1);
cmp1=(unsigned int)(m*ta+tb+tc);
cmp2=(unsigned int)ts2;
cmp3=0;
cmp4=(unsigned int)(m*ta+tb);
cmp5=0;
cmp6=(unsigned int)(m*ta);
}
else if(volts*sin_tab[i]>vdcbeta)
{
triangle=4;
ta=ts2*(2*kp*sin_tab[i]-1);
tb=(-kp)*ts*sin_tabn[i];
tc=ts*(1-kp*sin_tabp[i]);
cmp1=(unsigned int)(n*tc+ta+tb);
cmp2=(unsigned int)ts2;
cmp3=(unsigned int)(n*tc+ta);
cmp4=(unsigned int)ts2;
cmp5=0;
cmp6=(unsigned int)(n*tc);
}
else
{
triangle=3;
ta=ts2*(1-2*kp*sin_tab[i]);
tb=ts2*(2*kp*sin_tabp[i]-1);
tc=ts2*(1+2*kp*sin_tabn[i]);
if (ceta<pi6)
{
cmp1=(unsigned int)(m*ta+tb);
cmp2=(unsigned int)ts2;
cmp3=0;
cmp4=(unsigned int)(m*ta+tb+tc);
cmp5=0;
cmp6=(unsigned int)(m*ta);
}
else
{
cmp1=(unsigned int)(ta+tb+n*tc);
cmp2=(unsigned int)ts2;
cmp3=(unsigned int)(n*tc);
cmp4=(unsigned int)ts2;
cmp5=0;
cmp6=(unsigned int)(ta+n*tc);
}
}
}
/******************************************************************/
/*2 sector*/
/******************************************************************/
else if((ceta>=pi3)&&(ceta<2*pi3))
{
sector=2;
if(volts*sin_tab[i]<=vdcbeta)
{triangle=1;
ta=kp*ts*sin_tabp[i];
tb=ts2*(1-2*kp*sin_tab[i]);
tc=kp*ts*sin_tabn[i];
if (ceta<pi2)
{
cmp1=(unsigned int)(m*ta);
cmp2=(unsigned int)ts2;
cmp3=(unsigned int)(m*ta+tc);
cmp4=(unsigned int)ts2;
cmp5=0;
cmp6=(unsigned int)(m*ta+tb+tc);
}
else
{
cmp1=0;
cmp2=(unsigned int)(ta+tb+n*tc);
cmp3=(unsigned int)(n*tc);
cmp4=(unsigned int)ts2;
cmp5=0;
cmp6=(unsigned int)(tb+n*tc);
}
}
else if(volts*sin_tabp[i]>vdcbeta)
{
triangle=2;
ta=ts*(1-kp*sin_tab[i]);
tb=kp*ts*sin_tabn[i];
tc=ts2*(2*kp*sin_tabp[i]-1);
cmp1=(unsigned int)(m*ta+tc);
cmp2=(unsigned int)ts2;
cmp3=(unsigned int)(m*ta+tb+tc);
cmp4=(unsigned int)ts2;
cmp5=0;
cmp6=(unsigned int)(m*ta);
}
else if(volts*sin_tabn[i]>vdcbeta)
{
triangle=4;
ta=ts2*(2*kp*sin_tabn[i]-1);
tb=kp*ts*sin_tabp[i];
tc=ts*(1-kp*sin_tab[i]);
cmp1=0;
cmp2=(unsigned int)(n*tc+tb);
cmp3=(unsigned int)(n*tc+ta+tb);
cmp4=(unsigned int)ts2;
cmp5=0;
cmp6=(unsigned int)(n*tc);
}
else
{
triangle=3;
ta=ts2*(1-2*kp*sin_tabn[i]);
tb=ts2*(2*kp*sin_tab[i]-1);
tc=ts2*(1-2*kp*sin_tabp[i]);
if (ceta<pi2)
{
cmp1=(unsigned int)(m*ta);
cmp2=(unsigned int)ts2;
cmp3=(unsigned int)(m*ta+tb+tc);
cmp4=(unsigned int)ts2;
cmp5=0;
cmp6=(unsigned int)(m*ta+tc);
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -