Subversion Repositories svnkaklik

#include ".\camerus.h"

#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cteni z kamery

// kroutitka
#define  CERVENA  3  // AN3/RA3
#define  CERNA    2  // AN2/RA2
#define  ZELENA   1  // AN1/RA0
#define  MODRA    0  // AN0/RA1

// I/O
#define LED    PIN_C0      // LED signalizujici start programu
#define HREF   PIN_C5      // Signal HREF z kamery (v H po celou dobu radku)
#define PIX    PIN_C6      // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))
#define SERVO  PIN_B4      // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))
#define MOT_L  PIN_B5      // Smer otaceni leveho motoru; druhy pol je RC2
#define MOT_R  PIN_B6      // Smer otaceni praveho motoru; druhy pol je RC1

#define DATA   PIN_B2
#define CP     PIN_B1

void disp(int8 x)
{
   int n;

   x=~x;
   for(n=0;n<=7;n++)
   {
      if (x & 1 == 1) output_high(DATA); else output_low(DATA);
      output_high(CP);
      x>>=1;
      output_low(CP);
   }
}

void NightRider(int x)
{
   int n,i,j;

   for(j=0;j<x;j++)
   {
      i=0x01;
      for(n=0;n<7;n++)
      {
         disp(i);
         rotate_left(&i, 1);
         delay_ms(100);
      }
      for(n=0;n<7;n++)
      {
         disp(i);
         rotate_right(&i, 1);
         delay_ms(100);
      }
   }
   disp(i);
   delay_ms(100);
   i=0;
   disp(i);
}

void main()
{
   int8 cas;      // Cas hrany bila/cerna v radce
   int8 offset;   // Promena pro ulozeni ovsetu
   int8 rr;       // Promenna na ulozeni Rozumne rychlost
   int8 r1;       // Rychlost motoru 1
   int8 r2;       // Rychlost motoru 2

   int16 ble;     // Prodleva do rozjezdu

   setup_adc_ports(ALL_ANALOG);              // Zapnuti A/D prevodniku pro cteni kroutitek
   setup_adc(ADC_CLOCK_INTERNAL);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro mereni casu hrany W/B v radce
   setup_timer_1(T1_DISABLED);
   setup_timer_2(T2_DIV_BY_16,255,1);        // Casovac PWM motoru
   setup_ccp1(CCP_PWM); // RC1               // PWM pro motory
   setup_ccp2(CCP_PWM); // RC2
   setup_comparator(NC_NC_NC_NC);
   setup_vref(FALSE);

   set_tris_c(0b11111000);     // Nastaveni vstup/vystup pro branu C, protoze se nedela automaticky

   set_pwm1_duty(0);   // Zastav motory
   set_pwm2_duty(0);
   output_low(MOT_L);
   output_low(MOT_R);

   NightRider(1);    // Aby se poznalo, ze byl RESET
                     // Musi se pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy

   //... Nastaveni kamery ...
   i2c_start();      // Soft RESET kamery
   i2c_write(0xC0);        // Pro single slave musi mit vsechny zapisy adresu C0h
   i2c_write(0x12);        // Adresa registru COMH
   i2c_write(0x80 | 0x24); // Zapis ridiciho slova
   i2c_stop();

   i2c_start();      // BW
   i2c_write(0xC0);
   i2c_write(0x28);
   i2c_write(0b01000001);
   i2c_stop();

/*
   i2c_start();      // Contrast
   i2c_write(0xC0);
   i2c_write(0x05);
   i2c_write(0xA0);  // 48h
   i2c_stop();
*/
   i2c_start();      // Brightness
   i2c_write(0xC0);
   i2c_write(0x06);
   i2c_write(0x90);  // 80h
   i2c_stop();

/*
   i2c_start();      // Band Filter
   i2c_write(0xC0);
   i2c_write(0x2D);
   i2c_write(0x04 | 0x03);
   i2c_stop();
*/
   i2c_start();      // VSTRT
   i2c_write(0xC0);
   i2c_write(0x19);
   i2c_write(45);
   i2c_stop();

   i2c_start();      // VEND
   i2c_write(0xC0);
   i2c_write(0x1A);
   i2c_write(45);
   i2c_stop();

   NightRider(1);    // Aby se poznalo, ze byl RESET

   cas=128;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
   ble=0;

/*
while(true)
{
disp(ble);
   i2c_start();      
   i2c_write(0xC0);
   i2c_write(0x06);
   i2c_write(ble); 
   i2c_stop();
delay_ms(1000);
ble+=0x10;
};
*/

   // ... Hlavni smycka ...
   while(true)
   {
      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
      set_timer0(0);          // Vynuluj pocitadlo casu
      while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
      {
// Dodelat rozpoznani cerne cary napric pro zastaveni
// Pockat na shozeni signalu HREF
         if(!input(PIX))   // Pokud se dvakrat za sebou precetla CERNA
         if(!input(PIX))
         if(!input(PIX))
         {
            int8 pom;

            pom=get_timer0();    // Precti cas z citace casu hrany
//            cas=255-cas;         // Vlevo je vpravo
            if(pom<0b11000000) cas=pom;
            break;
         };
      };

      set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
      Delay_ms(1);
      offset=read_adc();

      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
      delay_us(1000);
      delay_us(offset);
      delay_us(offset);
      delay_us(cas);
      delay_us(cas);
      output_low(SERVO);

      ble++;                     // Casovac pro odlozeni rozjezdu po zapnuti napajeni
      set_adc_channel(ZELENA);   // Kroutitka pro vykon motoru (!!! to bude chtit jeste predelat !!!)
      Delay_ms(1);
      offset=read_adc();
      set_adc_channel(CERNA);
      Delay_ms(1);
      rr=read_adc();

disp(cas);

      r1=cas>>1;                 // Elektronicky diferencial
      r2=255-offset-(cas>>1);

      if (ble==99)               // Rozjezd na Rozumnou rychlost
      {
         set_pwm1_duty(rr);
         set_pwm2_duty(rr);
      };
      if ((ble>100)&&(ble<60000))      // Jizda
      {
         if (r1<=rr) set_pwm1_duty(r1);
         if (r2<=rr) set_pwm2_duty(r2);
      }
      else
      {
         set_pwm1_duty(0);       // Zastaveni
         set_pwm2_duty(0);
      }
   };
}