//Nataceni za Sluncem
//(c) Jan Chroust 2013


#include <main.h>

#define PIN_SDA  PIN_C4
#define PIN_SCL  PIN_C3
#use i2c(master, sda=PIN_SDA, scl=PIN_SCL)
#use rs232(baud=9600,parity=N,xmit=PIN_C7,rcv=PIN_C6,bits=8) //rcv TXD xmit RXD
#include <math.h>
#include <stdio.h>

#include <stdlib.h>
#include <input.c> 
#include <string.h>

//Akcelerometr
#define AK_W  0x38 //adresa akcelerometru zápis
#define AK_R  0x39 //adresa akcelerometru ètení
#define AK_XH 0x01 //osa X LSB
#define AK_XL 0x02 //osa X MSB
#define AK_YH 0x03 //osa Y LSB
#define AK_YL 0x04 //osa Y MSB
#define AK_ZH 0x05 //osa Z LSB
#define AK_ZL 0x06 //osa Z MSB

//Magnetometr
#define MAG_W  0x3C //adresa akcelerometru zápis
#define MAG_R  0x3D //adresa akcelerometru ètení
#define MAG_XH 0x03 //osa X LSB
#define MAG_XL 0x04 //osa X MSB
#define MAG_ZH 0x05 //osa Y LSB
#define MAG_ZL 0x06 //osa Y MSB
#define MAG_YH 0x07 //osa Z LSB
#define MAG_YL 0x08 //osa Z MSB

//pripojeni motoru
//AIN1 - pro vysku slunce
#define AIN1 PIN_D0 
#define AIN2 PIN_D1 
//motor A -cerveny vodic na AOUT1
//motor A -modry vodic na Aout2

//AIN2 - pro azimut
#define BIN1 PIN_D2 
#define BIN2 PIN_D3 
//motor B - cerveny vodic na BOUT2
//motor B - modrý vodic na BOUT1

signed int16 X, Y, Z,AX, AY, AZ; //promenne pro magnetometr a akcelerometr

unsigned int16 azimutZAD, elevaceZAD;

void setAK (void) //nastaveni akcelerometru
{
   i2c_start();
   I2C_Write(AK_W);
   I2C_write(0x2A);
   I2C_write(0x01); //nastavi aktivni stav
 
   i2c_stop(); 
}

void setmag (void)
{
  i2c_start();
  I2C_Write(MAG_W);     
  I2C_Write(0x00);   
  I2C_Write(0x78);
  i2c_stop();
  Delay_ms(6);
   
  i2c_start();
  I2C_Write(MAG_W);     
  I2C_Write(0x01);   
  I2C_Write(0x00);
  i2c_stop();

  Delay_ms(6);

  i2c_start();
  I2C_Write(MAG_W);     
  I2C_Write(0x02);   
  I2C_Write(0x00);
  i2c_stop();
  Delay_ms(6);  
}

int16 akR (int8 H, int8 L) //vycitani hodnot z akcelerometru
{
unsigned int8 XL=0,XH=0;
signed int16 x;

   i2c_start();
   I2C_Write(AK_W);
   I2C_write(H);
   i2c_start();
   I2C_Write(AK_R);
   XH=i2c_read(0);
   i2c_stop(); 

   i2c_start();
   I2C_Write(AK_W);
   I2C_write(L);
   i2c_start();
   I2C_Write(AK_R);
   XL=i2c_read(0);
   i2c_stop();
   
   x = (((unsigned int16) XH << 8) + XL ); //prevod na 16bit hodnotu
   x=x/4;
   
   return x;
}

int16 magR (int8 H, int8 L) //vycitani hodnot z magnetometru
{
unsigned int8 XL=0,XH=0;
signed int16 x;

   i2c_start();
   I2C_Write(MAG_W);
   I2C_write(H);
   i2c_start();
   I2C_Write(MAG_R);
   XH=i2c_read(0);
   i2c_stop(); 

   i2c_start();
   I2C_Write(MAG_W);
   I2C_write(L);
   i2c_start();
   I2C_Write(MAG_R);
   XL=i2c_read(0);
   i2c_stop();
   
  x = (((unsigned int16) XH << 8) + XL );

   
   return x;
}

float elevace (void) //vypocet aktualni elevace panelu
{
//printf("Akcelerometr5:  \r\n",);

X= akR (AK_XH, AK_XL);  
Y= akR (AK_YH, AK_YL); 
Z= akR (AK_ZH, AK_ZL); 

AX=abs(X);
AY=abs(Y)+250;
AZ=abs(Z)+250;

float a, b;
a=(float)Y/Z;
b=atan(a);
b = (b/3.14)*180;
b=abs(b);
   
if(((AX>AY) || (AX>AZ))) //indikace prevraceni panelu
   { 
   printf("Prevracený panel)\r\n", );
   }
else
   {
   if(Z==0) //osetreni proti deleni 0
   { 
      if(Y>0)
         { 
         b=180;
         }
      else
         {
         b=0;
         }
   }  
   else
      {
      if(Z>0)
       {
       if(Y>=0)
       {
         b=90+b;
       }
       else
       {
         b=90-b;
       }
       }
      else
       {
       if(Y>=0)
       {
         b=180-b;
       }
       else
       {
         b=270+b;
       }
       }
         
      }   
   
   
   
  }
  // printf("uhel namereny %10.2f \r\n", b);
   
   if(b>355)
   {
   b=0;
   }
else
   {

   }
      return b;
   
}

float azimut (void) //vypocet aktualni vysky panelu
{
X= magR (MAG_XH, MAG_XL);  
Y= magR (MAG_YH, MAG_YL); 
Z= magR (MAG_ZH, MAG_ZL); 




AX=abs(X);
AY=abs(Y);
AZ=abs(Z);

float a, b;
a=(float)Y/X;
b=atan(a);
b = (b/3.14)*180;
b=abs(b);
   

   

   if(X==0) //osetreni proti deleni 0
   { 
      if(Y>0)
         { 
         b=90;
         }
      else
         {
         b=270;
         }
   }  
   else
      {
      if(X>0)
       {
       if(Y>=0)
       {
         b=b;
       }
       else
       {
         b=360-b;
       }
       }
      else
       {
       if(Y>=0)
       {
         b=180-b;
       }
       else
       {
         b=180+b;
       }
       }
         
      } 
      
      
 if(b>355)
   {
   b=0;
   }
else
   {

   }

      return b;
   
}

void motorA (int8 H) //pro ovladani prvniho motoru - nastaveni vysky (elevace) solarniho panelu
{
switch(H){
   case 1:  //reverzní chod
   output_low (AIN2);
   output_high (AIN1);  
   break;

case 2: //dopredu
   output_low (AIN1);
   output_high (AIN2);   
   break;

default:
   output_low (AIN2);
   output_low (AIN1);
         }
}

void motorB (int8 H) //pro ovladani druheho motoru - nastveni nazimutu solarniho panelu
{
switch(H){
   case 1:  //reverzní chod
   output_low (BIN2);
   output_high (BIN1);  
   break;

case 2: //dopøedu
   output_low (BIN1);
   output_high (BIN2);   
   break;

default:
   output_low (BIN2);
   output_low (BIN1);
         }
}

void elevace_set (int16 H) //slouzi pro nastaveni nove vysky (elevace) panelu
{
//printf("Akcelerometr4:  \r\n",);
float a;
int16 b,c;

a=elevace();
b= (int16) a;
c=abs(H-b); 

while(c>2) //maximalni odchylka uhlu, aby nebylo potreba panelem hybat
   {
   while(H!=b) //probiha dokud se uhel panelu nerovna zadanemu na cele stupne
      {

      if(H>b)
         {
         motorA(2);
    
         }
      else
      {
      motorA(1);
      }

      delay_ms (50); //cas sepnuti motoru

      motorA(3); //vypne motor
      delay_ms (50); //doma na ustaleni panelu pred merenim
      a=elevace();
      b= (int16) a;
  
     
      c=abs(H-b);
      }
   }
motorA(3); //vypne motor
printf("Podarene nastaveni vysky: %Ld\r\n", b);


}



void azimut_set (int16 H) //slouzi pro nastaveni azimutu panelu
{
float a;
int16 b,c;


a=azimut();
b= (int16) a;
c=abs(H-b);

while(c>2) //maximalni odchylka uhlu, aby nebylo potreba panelem hybat
   {
   while(H!=b) //probiha dokud se uhel panelu nerovna zadanemu na cele stupne
      {

      if(H>b)
         {
         motorB(2);
     
         }
      else
      {
      motorB(1);
      }

      delay_ms (50); //cas sepnuti motoru

      motorB(3); //vypne motor
      delay_ms (50); //doma na ustaleni panelu pred merenim
      a=azimut();
      b= (int16) a;
           
      c=abs(H-b);
      }
   }
motorA(3); //vypne motor
printf("Podarene nastaveni azimutu: %Ld\r\n", b);

   

   }

void main()
{
   setup_adc_ports(NO_ANALOGS|VSS_VDD);
   setup_adc(ADC_CLOCK_DIV_2);
   setup_spi(SPI_SS_DISABLED);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
   setup_timer_1(T1_DISABLED);
   setup_timer_2(T2_DISABLED,0,1);
   setup_ccp1(CCP_OFF);
   setup_comparator(NC_NC_NC_NC);  

 
motorA(3); //vyponuti motoru
motorB(3);

setAK(); //nastaveni akcelerometru
setmag(); //nastveni magnetometru
printf("System nataceni panelu  \r\n",);
 
 while(TRUE)
 { 
int i[10];
int16 vysledek;
int b=0;
int c;


//nacteni hodnoty elevace ze seriove linky
  do {
       b++;  
       i[b] = getchar();
 
      // printf("Zadana elevace %d \r\n", i[b]);
      
   } while (i[b] != ' ');
b=b-1;

switch(b){
   case 1:  
   elevaceZAD=i[1]-48;
   break;

case 2: 
   elevaceZAD=(i[2]-48)+(i[1]-48)*10;
   break;
case 3: 
   elevaceZAD=(i[3]-48)+(i[2]-48)*10+((int16)i[1]-48)*100;
   break;   

default:
 
         }
printf("Zadaná elevace %Ld \r\n", elevaceZAD);         
         
i=0;
vysledek=0;
b=0;
c=0;

//nacteni hodnoty azimutu ze seriove linky
  do {
       b++;  
       i[b] = getchar();
 
       //printf("Zadany azimut %d \r\n", i[b]);
      
   } while (i[b] != ' ');
b=b-1;

switch(b){
   case 1:  
   azimutZAD=i[1]-48;
   break;

case 2: 
   azimutZAD=(i[2]-48)+(i[1]-48)*10;
   break;
case 3: 
   azimutZAD=(i[3]-48)+(i[2]-48)*10+((int16)i[1]-48)*100;
   break;   

default:
 
         }


printf("Zadany azimut %Ld \r\n", azimutZAD);
 
 
elevace_set (elevaceZAD); //pro nastveni nove elevace solarniho panelu
azimut_set(azimutZAD); //pro nastaveni noveho azimutu solarniho panelu

delay_ms (2000);
  
}
}