roboforum.ru

//ICC-AVR application builder : 13.09.2008 11:20:34
// Target : M128
// Crystal: 16.000Mhz


#include <iom128v.h>
#include <macros.h>
#include <math.h>





void port_init(void)
{
PORTA = 0xAA;
DDRA  = 0xFF;
PORTB = 0x00;
DDRB  = 0x00;
PORTC = 0x00; //m103 output only
DDRC  = 0x00;
PORTD = 0x00;
DDRD  = 0x00;
PORTE = 0x00;
DDRE  = 0x00;
PORTF = 0x00;
DDRF  = 0x00;
PORTG = 0x00;
DDRG  = 0x00;
}




//UART0 initialize
// desired baud rate: 9600
// actual: baud rate:9615 (0,2%)
// char size: 8 bit
// parity: Disabled
void uart0_init(void)
{
UCSR0B = 0x00; //disable while setting baud rate
UCSR0A = 0x00;
UCSR0C = 0x06;
UBRR0L = 0x67; //set baud rate lo
UBRR0H = 0x00; //set baud rate hi
UCSR0B = 0x98;
}

#pragma interrupt_handler uart0_rx_isr:iv_USART0_RXC
void uart0_rx_isr(void)
{
//uart has received a character in UDR
}




//call this routine to initialize all peripherals
void init_devices(void)
{
//stop errant interrupts until set up
CLI(); //disable all interrupts
XDIV  = 0x00; //xtal divider
XMCRA = 0x00; //external memory
port_init();

uart0_init();


MCUCR = 0x00;
EICRA = 0x00; //extended ext ints
EICRB = 0x00; //extended ext ints
EIMSK = 0x00;
TIMSK = 0x01; //timer interrupt sources
ETIMSK = 0x00; //extended timer interrupt sources
SEI(); //re-enable interrupts
//all peripherals are now initialized
}

//
void main(void)
{
int Phi;
float Phi0;
int U_k[4];
float A;
float B;
init_devices();
U_k[0]=100;
U_k[1]=50;
U_k[2]=60;
U_k[3]=40;
A=U_k[0]-U_k[2];
B=U_k[1]-U_k[3];
if ((A>B)&&(A>-B))  Phi0 = atan(B/A)*180/_PI,putchar0(1);
if ((A<B)&&(A>-B))  Phi0 =90- atan(A/B)*180/_PI,putchar0(2);
if ((A<B)&&(A<-B))  Phi0 =180+ atan(B/A)*180/_PI,putchar0(3);
if ((A>B)&&(A<-B))  Phi0 =270- atan(B/A)*180/_PI,putchar0(4);
Phi=(int)fround(Phi0);

putchar0(Phi>>8);
putchar0(Phi);


}






void putchar0(unsigned char data )//êîì1 ïåðåäà÷à
{
   
    while ( !( UCSR0A & (1<<UDRE0)) );// æäàòü ïîêà áóôåð íå îñâîáîäèòüñÿ
   
    UDR0 = data; // çïèñàòü â áóôåð
}
void putchar1(unsigned char data )//êîì2 ïåðåäà÷à
{
 
    while ( !( UCSR1A & (1<<UDRE1)) );
 
    UDR1 = data;
}



unsigned char getchar0( void )// êîì1 ïðèåì
{

while ( !(UCSR0A & (1<<RXC0)) );
return UDR0;
}
unsigned char getchar1( void )// êîì 2 ïðèåì
{

while ( !(UCSR1A & (1<<RXC1)) );
return UDR1;
}

const unsigned int arct[91]=
{0, 2, 3, 5, 7, 9, 11, 12, 14, 16, 18, 19, 21, 23, 25, 27, 29, 31, 32, 34, 36,
38, 40, 42, 45, 47, 49, 51, 53, 55, 58, 60, 62, 65, 67, 70, 73, 75, 78, 81,
84, 87, 90, 93, 97, 100, 104, 107, 111, 115, 119, 123, 128, 133, 138, 143,
148, 154, 160, 166, 173, 180, 188, 196, 205, 214, 225, 236, 248, 261, 275,
  290, 308, 327, 349, 373, 401, 433, 470, 514, 567, 631, 712, 814, 951, 1143,
   1430, 1908, 2864, 5729, 10000 };

unsigned char  arc (unsigned int D)
{

unsigned char i;
unsigned char toLeft=0;
unsigned char toRight=89;
unsigned char pol0=0;
unsigned char pol1=0;

   

while (1)//сам поиск
   {
   
   
      i=(toLeft+toRight) / 2;
      if (D < arct[i])
      {
         toRight= i-1;
         pol1 = i;
         
      }
      else if (D > arct[i])
      {
         toLeft= i+1;
         pol0=i;
         
      }
      if ((((pol0-pol1)<2)&&(pol0)&&(pol1))||(toLeft > toRight)||(D == arct[i]) ) break;
      
   }
return i;// позиция значения или что близкое
   
}

U[0..3] значения АЦП
unsigned int Phi=0;
unsigned int Phi_0;




if (((U[0])+(U[2])+(U[1])+(U[3]))>50)
{
if ((U[0]>U[2])&&(U[1]>=U[3]))
{if (U[1]==U[3]) Phi_0=10000;else
{
Phi_0=(((U[1]-U[3])*100)/((U[0]-U[2])));
if (Phi_0>6000) Phi=10000;
}
Phi=0;
}
if ((U[0]<=U[2])&&(U[1]>U[3]))
{if (U[0]==U[2]) Phi_0=10000;else
{
Phi_0=(((U[2]-U[0])*100)/((U[1]-U[3])));
if (Phi_0>6000) Phi=10000;
}
Phi=90;
}
if ((U[0]<U[2])&&(U[1]<=U[3]))
{if (U[1]==U[3]) Phi_0=10000;else
{
Phi_0=(((U[3]-U[1])*100)/((U[2]-U[0])));
if (Phi_0>6000) Phi=10000;
}
Phi=180;
}
if ((U[0]>=U[2])&&(U[1]<U[3]))
{if (U[0]==U[2]) Phi_0=10000;else
{
Phi_0=(((U[0]-U[2])*100)/((U[3]-U[1])));
if (Phi_0>6000) Phi=10000;
}
Phi=270;
}

Phi += arc(Phi_0);