#include "2407regs.h"
#include "float.h"
#include "math.h"
float ualfa[200],ubeta[200];
int sector[200];
#define PI2 2*3.1415926
#define DETA PI2/200
#define INIA 3.1415926/180
#define TP 1500
#define KP 0.7
void disable()
{
asm(" SETC INTM");
}
void initial()
{
*IFR=0XFFFF;
*IMR=0X0;
*SCSR1=0X81FE;
*WDCR=0XE8;
*T3PER=TP;
*T3CON=0X0802;
*ACTRB=0X666;
*COMCONB=0X9200;
*EVBIMRA=0X00;
*T3CNT=0X00;
*EVBIFRA=0X0FFFF;
*MCRC=*MCRC|0X7E;
WSGR=0X0000;
}
void calu()
{
int i;
for(i=0;i<200;i++)
{
ualfa[i]=KP*cos(INIA+i*DETA);
ubeta[i]=KP*sin(INIA+i*DETA);
}
}
void SECTOR()
{
int i,a,b,c;
float v1,v2,v3;
for(i=0;i<200;i++)
{
v1=ubeta[i];
v2=(-ubeta[i]+ualfa[i]*1.732051)/2;
v3=(-ubeta[i]-ualfa[i]*1.732051)/2;
if(v1>0) a=1;
else a=0;
if(v2>0) b=1;
else b=0;
if(v3>0) c=1;
else c=0;
a=4*c+2*b+a;
switch(a){
case 1: sector[i]=1;break;
case 2: sector[i]=5;break;
case 3: sector[i]=0;break;
case 4: sector[i]=3;break;
case 5: sector[i]=2;break;
case 6: sector[i]=4;break;
default: break;
}
}
}
main()
{
int anticlk[6]={0x1666,0x3666,0x2666,0x6666,0x4666,0x5666};
int i,k=0,cmp1,cmp2;
float x,y,z;
disable();
initial();
calu();
SECTOR();
while(1){
for(i=0;i<200;i++){
*ACTRB=anticlk[sector[i]];
x=ubeta[i];
y=(1.732051*ualfa[i]+ubeta[i])/2;
z=(-1.732051*ualfa[i]+ubeta[i])/2;
switch(sector[i]){
case 0: cmp1=(int)(-z*TP),cmp2=(int)(x*TP);break;
case 1: cmp1=(int)(y*TP), cmp2=(int)(z*TP);break;
case 2: cmp1=(int)(x*TP), cmp2=(int)(-y*TP);break;
case 3: cmp1=(int)(z*TP), cmp2=(int)(-x*TP);break;
case 4: cmp1=(int)(-y*TP),cmp2=(int)(-z*TP);break;
case 5: cmp1=(int)(-x*TP),cmp2=(int)(y*TP);break;
default: break;
}
*CMPR4=cmp1;
*CMPR5=cmp2;
if((i+k)==0) *T3CON=*T3CON|0x040;
while(1){
k=*EVBIFRA&0x0200;
if(k==0x0200) break;
}
}
}
}
void nothing()
{
return;
}
