#include "main.h"
// NEPOUZIVAT PINY B6 A B7, JSOU VYHRAZENY PRO SERIOVOU KOMUNIKACI
// BAUD RATE = 9600 Bd/s
// LED DIODY
#define  LED1     PIN_E0
#define  LED2     PIN_E1

// PIEZO
#DEFINE SOUND_HI   PIN_B4
#DEFINE SOUND_LO   PIN_B5

// RADKOVY SENZOR - OLSA
#define  SDIN     PIN_D4                  // seriovy vstup
#define  SDOUT    input(PIN_C5)           // seriovy vystup
#define  SCLK     PIN_D5                  // takt

#define  LEVEL_O  96                      // rozhodovaci uroven pro olsa - odpovida cerne care

// KOMUNIKACE S OLSA - prvni poslat LSB
int main_reset[8]={1,1,0,1,1,0,0,0};      // hlavni reset 0x1B
int set_mode_rg[8]={1,1,1,1,1,0,1,0};     // zapis do MODE registru 0x5F
int clear_mode_rg[8]={0,0,0,0,0,0,0,0};   // nulovani MODE registru 0x00

int left_offset[8]={0,0,0,0,0,0,1,0};     // offset leveho segmentu senzoru 0x40
int mid_offset[8]={0,1,0,0,0,0,1,0};      // offset prostredniho segmentu senzoru 0x42
int right_offset[8]={0,0,1,0,0,0,1,0};    // offset praveho segmentu senzoru 0x44
int offset[8]={1,0,0,0,0,0,0,1};          // minus jedna - pouzit pro vsechny segmenty 0x81

int left_gain[8]={1,0,0,0,0,0,1,0};       // zisk leveho segmentu 0x41
int mid_gain[8]={1,1,0,0,0,0,1,0};        // zisk leveho segmentu 0x43
int right_gain[8]={1,0,1,0,0,0,1,0};      // zisk leveho segmentu 0x45
int gain[8]={1,0,1,0,0,0,0,0};            // zisk = 5 - pouzit pro vsechny segmenty 0x5

int start_int[8]={0,0,0,1,0,0,0,0};       // zacatek integrace 0x08
int stop_int[8]={0,0,0,0,1,0,0,0};        // konec integrace 0x10
int readout[8]={0,1,0,0,0,0,0,0};         // cteni senzoru 0x02

int olsa_lseg[51]={0};                    // leva cast radky (pixely 0 - 50)
int olsa_rseg[51]={0};                    // prava cast radky (pixely 51 - 101)
int8  *lp;                                // ukazatel pro levou polovinu radky
int8  *rp;                                // ukazatel pro levou polovinu radky

//NARAZNIK
#define  BUMPL    input(PIN_D6)
#define  BUMPR    input(PIN_D7)

//MODRE SENZORY - nouzove
#define  LINEL    0
#define  LINER    1
#define  TRESHOLD 200                     // rozhodovaci uroven (zmereno 0 - cerna, 255 cerna)
int8  line_l;
int8  line_r;
int1  line_position;

// MOTORY - pro rychlost použity piny C1 (pravy motor) a C2 (levy motor)
#define  LMF   PIN_D0
#define  LMB   PIN_D1
#define  RMF   PIN_D2
#define  RMB   PIN_D3
// NASTAVENI RYCHLOSTI - pro vytvorene funkce
int8 lm_speed;                            
int8 rm_speed;

//PODPROGRAMY
//SENZORY
//OLSA01A
void  olsa_pulses(int count)              // vytvori impulzy pro ridici logiku
{
   int8 ct;
   for(ct=0;ct<=count;ct++)
   {
      output_high(SCLK);
      output_low(SCLK);
   }
} 

void olsa_pulse()                         // vytvori jeden impulz
{
   output_high(SCLK);
   output_low(SCLK);
}

void olsa_send(int8 info[8])              // USART komunikace s modulem OLSA01A - poslani zpravy
{
   int *ip;                               // ukazatel na pole s informaci
   int8 i;                                // pomocna promenna pro nastaveni 0 nebo 1 na SDIN
   output_low(SDIN);                      // start bit
   olsa_pulse();
   for(ip=0;ip<8;ip++)                    // predani informace - 8 bit, LSB prvni > MSB posledni
   {
      i=info[ip];                         // ziskani hodnoty z pole
      if(i==1)                            // vyhodnoceni obsahu informace - nastav 1
      {
         output_high(SDIN);
      }
      else                                // vyhodnoceni obsahu informace - nastav 0
      {
         output_low(SDIN);
      }
      olsa_pulse();   
   }
   output_high(SDIN);                     // stop bit
   olsa_pulse();
}

void olsa_reset()                         // hlavni RESET - provadi se po zapnuti
{
   output_low(SDIN);
   output_low(SCLK);
   olsa_pulses(30);                       // reset radkoveho senzoru
   output_high(SDIN);
   olsa_pulses(10);                       // start bit - synchronizace
   olsa_send(main_reset);
   olsa_pulses(5);
   olsa_send(set_mode_rg);
   olsa_send(clear_mode_rg);
}
   
void olsa_setup()                         // kompletni nastaveni, provadi se po resetu
{
   olsa_send(left_offset);                // nastaveni leveho segmentu (offset a zisk)
   olsa_send(offset);
   olsa_send(left_gain);
   olsa_send(gain);
   olsa_send(mid_offset);                 // nastaveni prostredniho segmentu (offset a zisk)
   olsa_send(offset);
   olsa_send(mid_gain);
   olsa_send(gain); 
   olsa_send(right_offset);               // nastaveni praveho segmentu (offset a zisk)
   olsa_send(offset);
   olsa_send(right_gain);
   olsa_send(gain); 
}
   
void olsa_integration()                   // snimani pixelu
{
   olsa_send(start_int);                  // zacatek integrace senzoru
   olsa_pulses(22);
   olsa_send(stop_int);                   // konec integrace senzoru
   olsa_pulses(5);
}

void read_olsa()
{
   int8  cpixel;                          // pocet prectenych pixelu
   int8  cbit;                            // pocet prectenych bitu
   int8  pixel;                           // hodnota precteneho pixelu      
   cpixel=0;
   lp=0;
   rp=0;
   olsa_integration();
   olsa_send(readout);
   do                                     // precte 102 pixelu
   {
      if(!SDOUT)                          // zacatek prenosu - zachycen start bit
      {  
         pixel=0;
         for(cbit=0;cbit<8;cbit++)        // cte jednotlive bity (8 bitu - 0 az 7)         
         {
            olsa_pulse();                 // impulz pro generovani dalsiho bitu
            if(SDOUT)                     // zachycena 1                        
            {
               pixel|=1;                  // zapise do bitu pixelu 1 - OR
            }
            else                          // zachycena 0
            {
               pixel|=0;                  // zapise do bitu pixelu 0 - OR
            }
            pixel<<=1;                    // posune pixel
         }
         olsa_pulse();                    // generuje stop bit
         if(cpixel<52)                    // ulozeni do pole
         {
            olsa_lseg[lp]=pixel;          // leva polovina radky - leve pole
            lp++;
         }
         else
         {
            olsa_rseg[rp]=pixel;          // prava polovina cary - prave pole
            rp++;
         }
         cpixel++;
      }
      else
      {
         olsa_pulse();                    // generuje start bit, nebyl-li poslan
      }
   }
   while(cpixel<102);                     // precte 102 pixelu
}

//ZACHRANNE SENZORY
void read_blue_sensors()                  // cteni nouzovych senzoru
{
   set_adc_channel(LINEL);                // cti levy nouzovy senzor
   delay_us(10);
   line_l=read_adc();     
   set_adc_channel(LINER);                // cti pravy nouzovy senzor
   delay_us(10);
   line_r=read_adc();
}
   
//PIPAK
void beep(int16 period,int16 length)
{
   int16 bp;                              //promenna pro nastaveni delky
   for(bp=length;bp>0;bp--)
   {
      output_high(SOUND_HI);
      output_low(SOUND_LO);
      delay_us(period);
      output_high(SOUND_LO);
      output_low(SOUND_HI);
      delay_us(period);
   }
}   
//MOTORY
void l_motor_fwd(int8 speedl)             // levy motor dopredu
{
   output_high(LMF);
   output_low(LMB);
   set_pwm2_duty(speedl);
}

void l_motor_bwd(int8 speedl)             // levy motor dozadu
{
   output_high(LMB);
   output_low(LMF);
   set_pwm2_duty(speedl);
}

void r_motor_fwd(int8 speedr)             // pravy motor dopredu
{
   output_high(RMF);
   output_low(RMB);
   set_pwm1_duty(speedr);
}

void r_motor_bwd(int8 speedr)             // pravy motor dozadu
{
   output_high(RMB);
   output_low(RMF);
   set_pwm1_duty(speedr);
}

void l_motor_off()                        // levy motor vypnut
{
   output_low(LMF);
   output_low(LMB);
   set_pwm2_duty(0);
}
   
void r_motor_off()                        // pravy motor vypnut
{  
   output_low(RMF);
   output_low(RMB);
   set_pwm1_duty(0);
}

void motor_test()                         // test motoru
{
   int8  i;
   beep(100,200);
   printf("TEST MOTORU\r\n");
   delay_ms(1000);
   printf("LEVY MOTOR DOPREDU\r\n");
   delay_ms(1000);
   for(i=0;i<255;i++)
   {
      l_motor_fwd(i);
      printf("RYCHLOST: %u\r\n",i);
      delay_ms(5);
   }
   for(i=255;i>0;i--)
   {
      l_motor_fwd(i);
      printf("RYCHLOST: %u\r\n",i);
      delay_ms(5);
   }   
   printf("LEVY MOTOR DOZADU\r\n");
   delay_ms(1000);
   for(i=0;i<255;i++)
   {
      l_motor_bwd(i);
      printf("RYCHLOST: %u\r\n",i);
      delay_ms(5);
   }   
   for(i=255;i>0;i--)
   {
      l_motor_bwd(i);
      printf("RYCHLOST: %u\r\n",i);
      delay_ms(5);
   }   
   printf("PRAVY MOTOR DOPREDU\r\n");
   delay_ms(1000);
   for(i=0;i<255;i++)
   {
      r_motor_fwd(i);
      printf("RYCHLOST: %u\r\n",i);
      delay_ms(5);
   }   
   for(i=255;i>0;i--)
   {
      r_motor_fwd(i);
      printf("RYCHLOST: %u\r\n",i);
      delay_ms(5);
   }   
   printf("PRAVY MOTOR DOZADU\r\n");
   delay_ms(1000);
   for(i=0;i<255;i++)
   {
      r_motor_bwd(i);
      printf("RYCHLOST: %u\r\n",i);
      delay_ms(5);
   }   
   for(i=255;i>0;i--)
   {
      r_motor_bwd(i);
      printf("RYCHLOST: %u\r\n",i);
      delay_ms(5);
   }
   l_motor_off();   
   r_motor_off();
   printf("KONEC TESTU MOTORU\r\n");
   delay_ms(1000);
}

void diag()                                  // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
{
   read_blue_sensors();
   printf("LEVA: %u \t",line_l);        
   printf("PRAVA: %u \t",line_r);   
   if(BUMPL)                                 // leva strana narazniku pusti test motoru
   {
      motor_test();
   }
   if(BUMPR)                                 // prava strana narazniku precte hodnoty z OLSA
   {
      int8  tisk;
      int8  *tiskp;
      read_olsa();                           // cte hodnoty z olsa a uklada do poli
      printf("\r\n");                        // po precteni vsech pixelu posle "enter"
      for(tiskp=0;tiskp<52;tiskp++)          // tisk leve casti radky
      {
         tisk=olsa_lseg[tiskp];
         printf("%x ",tisk);
      }
      for(tiskp=0;tiskp<52;tiskp++)          // tisk prave casti radky
      {
         tisk=olsa_rseg[tiskp];
         printf("%x ",tisk);
      }
   }
}

// HLAVNI SMYCKA
void main()                                  // NASTAVENI > provede se pouze pri zapnuti
{
   printf("POWER ON \r\n");
   setup_adc_ports(sAN0-sAN1-sAN2);          // pouzity analogove vstupy A0, A1 a A3
   setup_adc(ADC_CLOCK_INTERNAL);            // interni hodniny pro AD prevodnik
   setup_spi(SPI_SS_DISABLED);               // komunikace pres SPI zakazana
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  
   setup_timer_1(T1_DISABLED);               // casovac T1 nepouzit
   setup_timer_2(T2_DIV_BY_16,255,1);        // casovac T2 pro PWM
   setup_ccp1(CCP_PWM);                      // povoli PWM na pinu RC2
   setup_ccp2(CCP_PWM);                      // povolí PWM na pinu RC1
   setup_comparator(NC_NC_NC_NC);            // vypnuti vsech komparatoru
   setup_vref(FALSE);                        // nepouziva se referencni napeti
   l_motor_off();                            // vypne levy motor
   r_motor_off();                            // vypne pravy motor
   olsa_reset();                             // reset logiky radkoveho senzoru
   olsa_setup();                             // nastaveni segmentu radkoveho senzoru (offset a zisk)
   output_high(LED1);                        // zhasne LED1
   output_high(LED2);                        // zhasne LED2
   olsa_reset();                             // provede reset senzoru - nuno vzdy po zapnuti
   olsa_setup();                             // provede pocatecni nastaveni OLSA
   printf("Nastaveni provedeno! \r\n");
   beep(500,200);                            // pipni po ukonceni nastaveni
   printf("Zvolte mod. \r\n");
   while(true)                               // RIDICI SMYCKA
   {   
      read_olsa();
   }
}