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

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

   output_high(LED);    // Budeme blikat LED, aby se poznalo, ze byl RESET 

   delay_ms(200);       // 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);

   i2c_stop();

/*

   i2c_start();      // Brightness

   i2c_write(0xC0);

   i2c_write(0x06);

   i2c_write(0x80);

   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();

   output_low(LED); // Blikani LED

   delay_ms(200);

   output_high(LED); // Blikani LED

   delay_ms(200);

   output_low(LED);

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

   ble=0;

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

      {

         if(!input(PIX))   // Pokud se dvakrat za sebou precetla CERNA

         if(!input(PIX))

         {

            cas=get_timer0();    // Precti cas z citace casu hrany

            cas=255-cas;         // Vlevo je vpravo

            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();

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

      }

   };

}