#include "main.h"

// NEPOUZIVAT PINY B6 A B7, JSOU VYHRAZENY PRO SERIOVOU KOMUNIKACI
// BAUD RATE = 9600

// univerzalni LED diody
#define  LED1     PIN_A4
#define  LED2     PIN_A5

// piezo pipak
#DEFINE SOUND_HI   PIN_B4
#DEFINE SOUND_LO   PIN_B5

// radkovy senzor
#define  SDIN     PIN_D4                  // seriovy vstup
#define  SDOUT    input(PIN_B2)           // seriovy vystup
#define  SCLK     PIN_D5                  // takt

// pro komunikaci s OLSA, prvni se posila LSB
int1 main_reset[8]={1,1,0,1,1,0,0,0};      // hlavni reset 0x1B
int1 set_mode_rg[8]={1,1,1,1,1,0,1,0};     // zapis do MODE registru 0x5F
int1 clear_mode_rg[8]={0,0,0,0,0,0,0,0};   // nulovani MODE registru 0x00

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

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

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

int8 olsa_lseg[51]={0};                    // leva cast radky (pixely 0 - 50)
int8 olsa_rseg[51]={0};                    // prava cast radky (pixely 51 - 101)
int8 *line_lp=&olsa_lseg;                 // ukazatel na levou cast radky
int8 *line_rp=&olsa_rseg;                 // ukazatel na pravou cast radky
int8 pixel;                               // dec hodnota jednoho pixelu
int8 rpx;                                 // pocet prectenych pixelu
int8 rbit;

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

//nouzove senzory
#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
#define  LMF   PIN_D0
#define  LMB   PIN_D1
#define  RMF   PIN_D2
#define  RMB   PIN_D3

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++)
      {
      delay_us(1);               // doba pro ustaleni
      output_high(SCLK);
      delay_us(1);               // doba pro ustaleni
      output_low(SCLK);
      }
   } 

void olsa_pulse()                // vytvori jeden impulz
   {
   delay_us(1);                  // doba pro ustaleni
   output_high(SCLK);
   delay_us(1);                  // doba pro ustaleni
   output_low(SCLK);
   }

void olsa_send(int1 info[8]) // USART komunikace s modulem OLSA01A - poslani zpravy
   {
   int8 ip;                // ukazatel na pole s informaci
   int1 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);
   }

int1 bity[8] = { 0, 1, 1, 1, 0, 0, 1, 0 };
int8 bajt;

void bit_to_bajt()
{
   int i;
   
   bajt = 0;
   for (i = 0; i < 8; i++)
      bajt |= bity[i] << i;
   
   printf("bajt: %x\n", bajt);
}

void olsa_bit_save()             // ukladani jednotlivych bitu pixelu
   {
   t_bit_save:
      if(SDOUT==0)               // zacatek prenosu
         {
         do                      // prijimej zpravy
            {
            //program pro prijem
            rbit++;              // zapocita precteni bitu 0 - 7
            }
         while(rbit==7);
         goto e_bit_save;        // skoc na konec prenosu
         }
      else                       // posli impulz a cekej na prenos
         {
      olsa_pulse();
      goto t_bit_save;           // skoci na zacatek prenosu, pokud SDOUT == 1
         }
   e_bit_save:
      olsa_pulse();              // posle impulz pro generovani stop bitu
   }
   
void olsa_convert()
   {
   }

void olsa_byte_save()               // zapis pixelu do pole
   {
   if(rpx<=51)                      // leva polovina radky
      {
      olsa_lseg[line_lp]=pixel;     // zapis do pole
      line_lp++;
      }
   else                             // prava polovina radky
      {
      olsa_rseg[line_rp]=pixel;     // zapis od pole
      line_rp++;
      }
   }
   
void olsa_evaluate()                // vyhodnoceni polohy
   {
   }
   
void olsa_read()                    // precist senzor
   {
   rpx=0;                           // cteny pixel = 0
   line_lp=0;                       // ukazatel na levou cast radky = 0
   line_rp=0;                       // ukazatel na pravou cast radky = 0
   olsa_send(readout);              // prikaz pro cteni ze senzoru
   do
      {
      olsa_bit_save();              // precte a ulozi bity 
      olsa_convert();               // prevede bity do jednoho bytu
      olsa_byte_save();             // zapise byte do pole
      rpx++;
      }
   while(rpx==101);
   olsa_evaluate();                 // zjisti pozici cary
   }

//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");
   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");
   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");
   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");
   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);
      }
   printf("KONEC TESTU MOTORU\r\n");
   }

void diagnostika()                  // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
{
   read_blue_sensors();
   printf("LEVA: %u \t",line_l);       
   delay_ms(10); 
   printf("PRAVA: %u \t",line_r);
   delay_ms(10);   
   printf("L_NARAZ: %u \t",BUMPL);
   delay_ms(10);
   printf("P_NARAZ: %u \r\n",BUMPR);
   delay_ms(10);
   if(BUMPL&&BUMPR)                 // po zmacknuti stran narazniku spusti test motoru
   {
      motor_test();
   }
}

// HLAVNI SMYCKA
void main()
   {
   
   printf("POWER ON \r\n");
   // NASTAVENI > provede se pouze pri zapnuti
   setup_adc_ports(sAN0-sAN1-sAN2);         
   setup_adc(ADC_CLOCK_INTERNAL);            // interni hodniny pro AD prevodnik
   setup_spi(SPI_SS_DISABLED);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
   setup_timer_1(T1_DISABLED);
   setup_timer_2(T2_DIV_BY_16,255,1);        // casovac 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);
   setup_vref(FALSE);
   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
   beep(500,200);                            // pipni pri startu
   printf("OK! \r\n");
   delay_ms(500);
   printf("VYBRAT MOD... \r\n");
   
   while(true)
      {
      diagnostika();
      }
   }