compass.i

i2c通信

extern void  i2c_start(void);	    
extern char   i2c_write(char   a);	
extern char   i2c_read(void);		
extern void  i2c_stop(void);		

extern void twi_slave_init(char   add);

#pragma used+
sfrb TWBR=0;
sfrb TWSR=1;
sfrb TWAR=2;
sfrb TWDR=3;
sfrb ADCL=4;
sfrb ADCH=5;
sfrw ADCW=4;      
sfrb ADCSRA=6;
sfrb ADMUX=7;
sfrb ACSR=8;
sfrb UBRRL=9;
sfrb UCSRB=0xa;
sfrb UCSRA=0xb;
sfrb UDR=0xc;
sfrb SPCR=0xd;
sfrb SPSR=0xe;
sfrb SPDR=0xf;
sfrb PIND=0x10;
sfrb DDRD=0x11;
sfrb PORTD=0x12;
sfrb PINC=0x13;
sfrb DDRC=0x14;
sfrb PORTC=0x15;
sfrb PINB=0x16;
sfrb DDRB=0x17;
sfrb PORTB=0x18;
sfrb EECR=0x1c;
sfrb EEDR=0x1d;
sfrb EEARL=0x1e;
sfrb EEARH=0x1f;
sfrw EEAR=0x1e;   
sfrb UBRRH=0x20;
sfrb UCSRC=0X20;
sfrb WDTCR=0x21;
sfrb ASSR=0x22;
sfrb OCR2=0x23;
sfrb TCNT2=0x24;
sfrb TCCR2=0x25;
sfrb ICR1L=0x26;
sfrb ICR1H=0x27;
sfrw ICR1=0x26;   
sfrb OCR1BL=0x28;
sfrb OCR1BH=0x29;
sfrw OCR1B=0x28;  
sfrb OCR1AL=0x2a;
sfrb OCR1AH=0x2b;
sfrw OCR1A=0x2a;  
sfrb TCNT1L=0x2c;
sfrb TCNT1H=0x2d;
sfrw TCNT1=0x2c;  
sfrb TCCR1B=0x2e;
sfrb TCCR1A=0x2f;
sfrb SFIOR=0x30;
sfrb OSCCAL=0x31;
sfrb TCNT0=0x32;
sfrb TCCR0=0x33;
sfrb MCUCSR=0x34;
sfrb MCUCR=0x35;
sfrb TWCR=0x36;
sfrb SPMCR=0x37;
sfrb TIFR=0x38;
sfrb TIMSK=0x39;
sfrb GIFR=0x3a;
sfrb GICR=0x3b;
sfrb SPL=0x3d;
sfrb SPH=0x3e;
sfrb SREG=0x3f;
#pragma used-

#asm
	#ifndef __SLEEP_DEFINED__
	#define __SLEEP_DEFINED__
	.EQU __se_bit=0x80
	.EQU __sm_mask=0x70
	.EQU __sm_powerdown=0x20
	.EQU __sm_powersave=0x30
	.EQU __sm_standby=0x60
	.EQU __sm_ext_standby=0x70
	.EQU __sm_adc_noise_red=0x10
	.SET power_ctrl_reg=mcucr
	#endif
#endasm

#pragma used+

unsigned char cabs(signed char x);
unsigned int abs(int x);
unsigned long labs(long x);
float fabs(float x);
int atoi(char *str);
long int atol(char *str);
float atof(char *str);
void itoa(int n,char *str);
void ltoa(long int n,char *str);
void ftoa(float n,unsigned char decimals,char *str);
void ftoe(float n,unsigned char decimals,char *str);
void srand(int seed);
int rand(void);
void *malloc(unsigned int size);
void *calloc(unsigned int num, unsigned int size);
void *realloc(void *ptr, unsigned int size); 
void free(void *ptr);

#pragma used-
#pragma library stdlib.lib

#pragma used+

signed char cmax(signed char a,signed char b);
int max(int a,int b);
long lmax(long a,long b);
float fmax(float a,float b);
signed char cmin(signed char a,signed char b);
int min(int a,int b);
long lmin(long a,long b);
float fmin(float a,float b);
signed char csign(signed char x);
signed char sign(int x);
signed char lsign(long x);
signed char fsign(float x);
unsigned char isqrt(unsigned int x);
unsigned int lsqrt(unsigned long x);
float sqrt(float x);
float floor(float x);
float ceil(float x);
float fmod(float x,float y);
float modf(float x,float *ipart);
float ldexp(float x,int expon);
float frexp(float x,int *expon);
float exp(float x);
float log(float x);
float log10(float x);
float pow(float x,float y);
float sin(float x);
float cos(float x);
float tan(float x);
float sinh(float x);
float cosh(float x);
float tanh(float x);
float asin(float x);
float acos(float x);
float atan(float x);
float atan2(float y,float x);

#pragma used-
#pragma library math.lib

unsigned char BeEEPROM[10];

unsigned char slarv=0;

unsigned char adrpoint=33;

unsigned char state=0;   

interrupt [18] void twi_isr(void)
{

TWCR&=0X7F;  

state=TWSR&0xF8;

switch (state)
{

case 0x60	: 
case 0x68	:
case 0x70	:
case 0x78	:
slarv=0; 
{TWCR=TWCR&0x0F |(1<<6)|(1<<7);};  
break;
case 0x80	:
case 0x88	: 

if(slarv==0)
{   
adrpoint = TWDR; 
slarv = 1;
}
else
{   BeEEPROM[adrpoint]=TWDR;

slarv=0;
adrpoint++;
}    

{TWCR=TWCR&0x0F |(1<<6)|(1<<7);};  
break;
case 0x90	:
case 0x98	:	

{TWCR=TWCR&0x0F |(1<<6)|(1<<7);};  
break;
case 0xA0	:

{TWCR=TWCR&0x0F |(1<<6)|(1<<7);}; 
break;

case 0xA8	:               

case 0xB0	:

case 0xB8	:            

if(adrpoint!=33)
TWDR=BeEEPROM[adrpoint]; 
{TWCR=TWCR&0x0F |(1<<6)|(1<<7);};  

break;    
case 0xC0	:            
case 0xC8	:            

{TWCR=TWCR&0x0F |(1<<7);}; 
twi_slave_init(0xd0);        
break; 
case 0xF8	:                 

break; 
case 0x00	:               

TWCR=TWCR&0x0F |(1<<7)|(1<<4)|(1<<6); 
break;  
default:   

break;
}
}    

void twi_slave_init(char   add)
{
TWCR= 0x00; 
TWBR= 0x00; 
TWSR= 0x00; 
TWAR= add; 
TWCR= 0x45; 

} 

void EEPROM_Write(unsigned int uiAddress, unsigned char ucData)
{
while(EECR & (1<<1 )) ;
EEAR = uiAddress;
EEDR = ucData;
EECR |= (1<<2 );
EECR = (1<<1 );    
}

unsigned char EEPROM_Read(unsigned int uiAddress)
{
while(EECR & (1<<1 )) ; 
EEAR = uiAddress;
EECR |= (1<<0 );  
return EEDR;
}
#pragma asm_function+
void EEPROM_Write_W(unsigned int uiAddress, unsigned int ucData)
{
#asm
       ld    r22,y+
       ld    r23,y+
       ld    r24,y+
       ld    r25,y+
       ser   r31
       in    r0,0x3f
__wait:
       sbic  0x1c,1
       rjmp  __wait
       out   0x1f,r25
       out   0x1e,r24
       out   0x1d,r22
       cli
       sbi   0x1c,2
       sbi   0x1c,1
       out   0x3f,r0
       sbrs  r31,0
       ret
       subi  r24,0xff
       sbci  r25,0xff
       mov   r22,r23
       clr   r31
       rjmp  __wait
   #endasm

}
#pragma asm_function-

#pragma asm_function+
unsigned int EEPROM_Read_W(unsigned int uiAddress)
{
#asm
       ld    r30,y+
       ld    r31,y+
__wait:
       sbic  0x01,1
       rjmp  __wait
       out   0x1f,r31
       out   0x1e,r30
       sbi   0x1c,0
       adiw  r30,0x01
       in    r0,0x1d
       out   0x1f,r31
       out   0x1e,r30
       sbi   0x1c,0
       mov   r30,r0
       in    r31,0x1d
       ret
   #endasm
}
#pragma asm_function-

unsigned int read_adc(unsigned char adc_input)
{
ADMUX=adc_input|0x00 ;

ADCSRA|=0x40;

while ((ADCSRA & 0x10)==0);
ADCSRA|=0x10;
return ADCW;
}

void SYS_INIT(void)    
{

TCCR0=0x00;

TCNT0=0;

TIMSK=0x00;

TCCR1A=0x00;
TCCR1B=0x00;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;

ASSR=0x00;
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;

MCUCR=0x00;

ACSR=0x80;
SFIOR=0x00;   
}

unsigned int adcustore[15];
unsigned int adcmstore[12];

unsigned int adcustore2[15];
unsigned int adcmstore2[12];

unsigned int arcsin(unsigned int x)
{   unsigned int px;
unsigned int r; 
px=x;

if((px>=0)&&(px<=2500))  r=(px+86)/86;                
else if((px>2500)&&(px<=4300)) r=29+(px-2500+83)/83;    
else if((px>4300)&&(px<=5800)) r=51+(px-4300+76)/76;    
else if((px>5800)&&(px<=7100)) r=71+(px-5800+67)/67;    
else if((px>7100)&&(px<=7700)) r=91+(px-7100+59)/59;    
else if((px>7700)&&(px<=8200)) r=101+(px-7700+53)/53;    
else if((px>8200)&&(px<=8700)) r=111+(px-8200+47)/47;    
else if((px>8700)&&(px<=9100)) r=121+(px-8700+40)/40;  
else if((px>9100)&&(px<=9400)) r=131+(px-9100+33)/33; 
else if(px>9400) r=180;                               
r=r>>1;

return r;   
}

void lightled(unsigned char num)
{
if((num%2)==1) 
{PORTB&=0xfc;PORTB|=0x02;PORTD=~(0x01<<((num+1)/2-1));}
else 
{PORTB&=0xfc;PORTB|=0x01;PORTD=~(0x01<<((num+2)/2-1));}
}

void delayms(unsigned char ii)
{
unsigned char i=20;
while(ii--)
{
while(i--);
}  
}  

void delayms2(unsigned char ii)
{
unsigned int i=800;
while(ii--)
{
while(i--);
}  
}  

unsigned char delayswitch(void)
{  unsigned int  ii;
ii=0;
PORTB|=0x01;
PORTB&=0xfd;
delayms2(1);
while(!(PINC&0x08))

{  delayms2(1);
ii++;
if(ii>4500) 
{
return 0;
}                                               
else if((ii>3000)&&(ii<=4500)) {PORTD=0xf8; }  
else if((ii>1500)&&(ii<=3000)) {PORTD=0xfc; }
else if((ii>20)&&(ii<=1500)) {PORTD=0xfe; }

}
PORTB&=0xfe; 
if((ii>1500)&&(ii<=4500)) {return 2; }  

else if((ii>20)&&(ii<=1500)) {return 1; }
return 0;

}

static void io_init(void)
{
PORTD = 0xff;
DDRD = 0xff;
PORTC =0xff;
DDRC=0x00;
PORTB =0x00;
DDRB=0xff;
}

void main(void)
{  unsigned int adcuadd,adcmadd,adc6add; 
unsigned int adcuadd2,adcmadd2,adc7add;
unsigned int R,R2,Rf,Rs,Rd,Re;
unsigned char delaytime,countdelay;
unsigned int countnum0,countnum1;
unsigned int i,j,k,uu6,uu7;
unsigned char *cpEEP,sig,sigOTC,sigSTC;
float cangle6,cangle7;
unsigned int icangle6,icangle7;
float A6;
float A7;
unsigned int offset,poffset,lastlight;
unsigned int adc6min;
unsigned int adc6max;
unsigned int adc7min;
unsigned int adc7max;
unsigned int adc6mid;
unsigned int adc7mid;
unsigned int offsetR;

#asm("sei")
SREG&=0x7f;   
delayms(5);   
SYS_INIT();     
io_init();     
ADMUX=(0x00);ADCSRA=(0x83); 
(cpEEP=BeEEPROM);                    
twi_slave_init(0xd0); 

countnum0=0;
countnum1=0;
adcuadd=0;
adcmadd=0;

adcuadd2=0;
adcmadd2=0;

adc6min=900;
adc6max=300;
adc7min=900;
adc7max=300;
adc6mid=600;
adc7mid=600;
sig=1;
sigOTC=0;
sigSTC=0;
countdelay=0;

adc6min=EEPROM_Read(0)*256+EEPROM_Read(1);
adc6max=EEPROM_Read(2)*256+EEPROM_Read(3);
adc7min=EEPROM_Read(4)*256+EEPROM_Read(5);
adc7max=EEPROM_Read(6)*256+EEPROM_Read(7);

offsetR=EEPROM_Read(8)*256+EEPROM_Read(9);
offset=EEPROM_Read(10)*256+EEPROM_Read(11);
lastlight=0;

A6=(float)(adc6max-adc6min)/2.0;  
A7=(float)(adc7max-adc7min)/2.0; 
adc6mid=(adc6min+adc6max)/2;  
adc7mid=(adc7min+adc7max)/2;

if(offset>15)     
offset=2;
if(offsetR>359)
offsetR=0;
read_adc(6);read_adc(7);    
delayms(5);               
SREG|=0x80; 

while(1)
{   
if(countnum0==15)                 
{    countnum0=0;sigOTC=1;           
for(i=0;i<14;i++)                
for(j=i+1;j<15;j++) 
{
if(adcustore[i]>adcustore[j])
{  
k=adcustore[j];
adcustore[j]=adcustore[i];
adcustore[i]=k;  

}
if(adcustore2[i]>adcustore2[j])
{  
k=adcustore2[j];
adcustore2[j]=adcustore2[i];
adcustore2[i]=k;  

}
}

for(i=7;i<8;i++)                          
{
adcuadd=adcuadd+adcustore[i];
adcuadd2=adcuadd2+adcustore2[i];
}
adcmstore[countnum1]=adcuadd;           
adcmstore2[countnum1]=adcuadd2; 
adcuadd=0;  adcuadd2=0;
countnum1++;   
} 
else 
{  
SREG&=0x7f;                               

adcustore[countnum0]= read_adc(6);        

adcustore2[countnum0++]=read_adc(7);
SREG|=0x80;                                    
}   

if((countnum1==12)&&sigOTC)                    
{     sigSTC=1; sigOTC=0;                     
countdelay++;
if( countdelay==6)  countdelay=0;

for(i=0;i<12;i++)                    
{
adcmadd=adcmadd+adcmstore[i]; 
adcmadd2=adcmadd2+adcmstore2[i];
}
adcmadd=adcmadd/12; 
adcmadd2=adcmadd2/12;

countnum1=0;                           

cangle6=(float)(adcmadd-adc6mid);
cangle7=(float)(adcmadd2-adc7mid);

if(cangle6>=0)
icangle6= (unsigned int)(cangle6*10000/A6);
else  icangle6= (unsigned int)((-cangle6)*10000/A6);
if(cangle7>=0)
icangle7= (unsigned int)(cangle7*10000/A7);
else  icangle7= (unsigned int)((-cangle7)*10000/A7);

if((cangle6>0)&&(cangle7>0))     { R=90-arcsin(icangle6);R2=arcsin(icangle7);   }     

else if((cangle6>0)&&(cangle7<0)) 
{ 
if(arcsin(icangle6)==90) R=359;  else R=270+arcsin(icangle6); 

R2=360-arcsin(icangle7);
}
else if((cangle6<0)&&(cangle7>0)) { R=90+arcsin(icangle6); R2=180-arcsin(icangle7); }
else if((cangle6<0)&&(cangle7<0)) { R=270-arcsin(icangle6); R2=180+arcsin(icangle7);}
else if((cangle6==0)&&(cangle7>0))  {R=90;R2=90; }
else if((cangle6==0)&&(cangle7>0))  {R=270;R2=270; }
else if((cangle6>0)&&(cangle7==0))  {R=0;R2=0; }
else if((cangle6<0)&&(cangle7==0))  {R=180;R2=180; }    

if(((R2>=0)&&(R2<=40))||((R2>=320)&&(R2<=360))||((R2>=140)&&(R2<=220)))
{  if(fabs(R-R2)>12)
Rf=R2;
else if(fabs(R-R2)>10)
Rf=(R*1+R2*9)/10;
else if(fabs(R-R2)>8)
Rf=(R*2+R2*8)/10;
else if(fabs(R-R2)>6)
Rf=(R*3+R2*7)/10;
else if(fabs(R-R2)>4)
Rf=(R*4+R2*6)/10;
else if(fabs(R-R2)>2)
Rf=(R*9+R2*11)/20;

else Rf=(R+R2)/2;
}
else if(((R>=40)&&(R<=130))||((R>=230)&&(R<=310)))
{
if(fabs(R-R2)>12)
Rf=R;
else if(fabs(R-R2)>10)
Rf=(R*9+R2*1)/10;
else if(fabs(R-R2)>8)
Rf=(R*8+R2*2)/10;
else if(fabs(R-R2)>6)
Rf=(R*7+R2*3)/10;
else if(fabs(R-R2)>4)
Rf=(R*6+R2*4)/10;
else if(fabs(R-R2)>4)
Rf=(R*9+R2*11)/20;
else Rf=(R+R2)/2;
}
else

{
Rf=(R+R2)/2;
}

if(Rf>=360) Rf=359;        

if(Rf<offsetR) Re=Rf+360-offsetR;
else Re=Rf-offsetR;

*cpEEP=(Re/256);
*(cpEEP+1)=(Re%256); 

uu6=adcmadd;
uu7=adcmadd2;

if(((uu6<adc6min)||(uu7<adc7min)||(uu6>adc6max)||(uu7>adc7max))&&sigSTC)
{
if(uu6<adc6min)
adc6min=uu6+1;
else if(uu6>adc6max)
adc6max=uu6-1;

if(uu7<adc7min)
adc7min=uu7+1;
else if(uu7>adc7max)
adc7max=uu7-1;

A6=(float)(adc6max-adc6min)/2.0;  
A7=(float)(adc7max-adc7min)/2.0; 
adc6mid=(adc6min+adc6max)/2;  
adc7mid=(adc7min+adc7max)/2; 
}

adc6add=adcmadd;
adc7add=adcmadd2;
adcmadd=0;  adcmadd2=0;   
Rs=Rf*4;

Rd=Rs/90;
if(sig==1)
{  
if(Rd<=offset) 
{  if((offset-Rd)<=13)
poffset=2+offset-Rd;
else 
poffset=offset-Rd-14; 

}
else 
{   if((Rd-offset)<=2)
poffset=2-(Rd-offset);
else
poffset=18-(Rd-offset);
}
if(poffset>15)  poffset=lastlight;
lightled(poffset);            
lastlight=poffset;
}

PORTB|=4;
delayms(2);
PORTB&=0xfb;  
delayms(8);
PORTB|=4;
delayms(8);

}

if((PINC&0x08)&&(sig==2))
{
if(sigSTC)
{
uu6=adc6add;
uu7=adc7add;
if(uu6<adc6min)
adc6min=uu6-2;

else if(uu6>adc6max)
adc6max=uu6+2;

if(uu7<adc7min)
adc7min=uu7-2;
else if(uu7>adc7max)
adc7max=uu7+2;

A6=(float)(adc6max-adc6min)/2.0;  
A7=(float)(adc7max-adc7min)/2.0; 
adc6mid=(adc6min+adc6max)/2;  
adc7mid=(adc7min+adc7max)/2; 

}

if(countdelay==0)
{
PORTB|=0x01;
PORTB&=0xfd;
PORTD=0x00;
} 
else if(countdelay==3)
{   

PORTD=0xff;

}

}

if((!(PINC&0x08))&&(sig==2))
{
delaytime=delayswitch();    
if(delaytime>=1)
{ PORTB|=0x03;

PORTD=0x00;
sig=1;
SREG&=0X7F;
delayms(1);
EEPROM_Write(0, (adc6min/0xff));
EEPROM_Write(1, (adc6min%0xff));
EEPROM_Write(2, (adc6max/0xff));
EEPROM_Write(3, (adc6max%0xff));
EEPROM_Write(4, (adc7min/0xff));
EEPROM_Write(5, (adc7min%0xff));
EEPROM_Write(6, (adc7max/0xff));
EEPROM_Write(7, (adc7max%0xff));
delayms(1);
PORTB&=0xfc;
SREG|=0X80;

}

} 

else if((!(PINC&0x08))&&(sig==1))
{
delaytime=delayswitch();
if(delaytime==1)
{   

offset=Rd;
offsetR=Rf;

SREG&=0x7f;
EEPROM_Write(8, (offsetR/0xff));
EEPROM_Write(9, (offsetR%0xff));
EEPROM_Write(10, (offset/0xff));
EEPROM_Write(11, (offset%0xff));
delayms(1);
SREG|=0X80; 

sig=1;
} 
else if(delaytime>=2)
{PORTB|=0x01;

PORTB&=0xfd;
PORTD=0x00;

sig=2;

adc6min=900;
adc6max=300;
adc7min=900;
adc7max=300;
adc6mid=500;
adc7mid=500;
}  

delaytime=0;
}

sigOTC=0;
sigSTC=0;      
}
}