#include "main.h"

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

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

//piezo pipak
#DEFINE SOUND_HI   PIN_B1
#DEFINE SOUND_LO   PIN_B2

//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;

// motory
#define  LMF   PIN_D0
#define  LMB   PIN_D1
#define  RMF   PIN_D2
#define  RMB   PIN_D3

int16 bl;
//PODPROGRAMY
//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(unsigned int16 period, unsigned int16 length)
{
   unsigned 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;
   printf("TEST MOTORU\n");
   delay_ms(1000);
   for(i=0;i<255;i++)
   {
      l_motor_fwd(i);
      printf("RYCHLOST: %u\n",i);
      delay_ms(10);
   }
   for(i=255;i>0;i--)
   {
      l_motor_fwd(i);
      printf("RYCHLOST: %u\n",i);
      delay_ms(10);
   }
   for(i=0;i<255;i++)
   {
      l_motor_bwd(i);
      printf("RYCHLOST: %u\n",i);
      delay_ms(10);
   }
   
   for(i=255;i>0;i--)
   {
      l_motor_bwd(i);
      printf("RYCHLOST: %u\n",i);
      delay_ms(10);
   }
   for(i=0;i<255;i++)
   {
      r_motor_fwd(i);
      delay_ms(10);
   }
   
   for(i=255;i>0;i--)
   {
      r_motor_fwd(i);
      printf("RYCHLOST: %u\n",i);
      delay_ms(10);
   }
   for(i=0;i<255;i++)
   {
      r_motor_bwd(i);
      printf("RYCHLOST: %u\n",i);
      delay_ms(10);
   }
   
   for(i=255;i>0;i--)
   {
      r_motor_bwd(i);
      printf("RYCHLOST: %u\n",i);
      delay_ms(10);
   }
   l_motor_off();
   r_motor_off();
}

void diagnostika()
{
   read_blue_sensors();
   printf("LEVA: %u \t",line_l);       
   delay_ms(20); 
   printf("PRAVA: %u \t",line_r);
   delay_ms(20);   
   printf("L_NARAZ: %u \t",BUMPL);
   delay_ms(20);
   printf("P_NARAZ: %u \n",BUMPR);
   delay_ms(20);
   if(BUMPL&&BUMPR)
   {
      motor_test();
   }
}

// HLAVNI SMYCKA
void main()
{
   printf("POWER ON \n");
   // NASTAVENI > provede se pouze pri zapnuti
   setup_adc_ports(sAN0-sAN1-sAN2);         
   setup_adc(ADC_CLOCK_INTERNAL);
   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);
   output_high(LED1);                  // zhasne LED1
   output_high(LED2);                  // zhasne LED2
   l_motor_off();                      // vypne oba motory
   r_motor_off();                      // vypne oba motory
   printf("OK! \n");
   printf("VYBRAT MOD... \n");
   while(true)
   {
      bl++;                            // primitivni blikani - oznacuje vypber modu
      if(bl>4096)
      {
         output_low(LED1);
         output_high(LED2);
      }
      else
      {
         output_high(LED1);
         output_low(LED2);
      }
      if(bl>8192)
      {
         bl=0;
      }
      if(BUMPL)
      {
         printf("MOD: DIAGNOSTIKA\n");
         output_low(LED1);
         output_high(LED2);
         delay_ms(1000);
         while(true)
         {
            diagnostika();
         }
      }
      if(BUMPR)
      {
         printf("MOD: STOPOVANI\n");
         output_low(LED2);
         output_high(LED1);
         delay_ms(1000);
         while(true)
         {        

         }
      }  
  }
}