WebSVN – svnkaklik – Diff – /roboti/istrobot/camerus/SW/876/camerus.c


//********* Robot Camerus pro IstRobot 2007 ************
//"$Id: camerus.c 236 2007-04-17 14:40:34Z kakl $"
//*****************************************************
 
#include ".\camerus.h"
 
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
 
// Rychlostni konstanty
#define RR_CIHLA     20       // Rozumna rychlost pro objizdeni cihly
#define RR_PRERUSENI 20       // Rozumna rychlost pro priblizeni se k preruseni
#define BRZDNA_DRAHA 8        // Jak daleko pred problemem se zacne brzdit
 
#define CAMERA_ADR   0xC0
#define SONAR_ADR    0xE0
 
// A/D vstupy
#define  RMAX     4  // AN4/RA5 - leve cidlo na vyjeti z cary
#define  LMAX     3  // AN3/RA3 - prave cidlo na vyjeti z cary
#define  CERVENA  2  // AN2/RA2 - cervene kroutitko
#define  ZELENA   1  // AN1/RA0 - zelene kroutitko
#define  MODRA    0  // AN0/RA1 - modre kroutitko
 
// I/O
#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 MOT_1     PIN_C1      // PWM vystpy motoru
#define MOT_2     PIN_C2      //
#define DATA      PIN_B2      // K modulu LEDbar data
#define CP        PIN_B1      // K modulu LEDbar hodiny
//#define ODO       PIN_C0      // Ze snimace z odometrie z praveho kola na TIMER1
                                // Jeden impuls je 31,25mm
#define IRRX      !input(PIN_B0)   // Vstup INT, generuje preruseni pri prekazce
#define IRTX      PIN_B3      // Modulovani vysilaci IR LED na detekci prekazky
#define PROXIMITY PIN_C7      // Cidlo kratkeho dosahu na cihlu
#define BUMPER    !input(PIN_A4)  // Naraznik
 
#define CASMIN 6           // Rozsah radku snimace
#define CASMAX 192
#define CASAVR ((CASMAX+CASMIN) / 2)
 
#define EEMAX  255         // Konec EEPROM
#define MAXLOG 0x30        // Maximalni pocet zaznamu v logu
#if  MAXLOG>(EEMAX/2-1)
   #error  Prekrocena velikost EEPROM
#endif
 
#define OFFSETO   0x9F //0x9F         // Vystredeni serva pro objeti prekazky
 
#define THR 90    // Threshold pro UV cidla na caru
 
#byte INTCON      = 0x0B         // Interrupt configuration register
   #bit GIE       = INTCON.7
   #bit PEIE      = INTCON.6
   #bit TMR0IE    = INTCON.5
   #bit INT0IE    = INTCON.4
   #bit RBIE      = INTCON.3
   #bit TMR0IF    = INTCON.2
   #bit INT0IF    = INTCON.1
   #bit RBIF      = INTCON.0
 
enum stavy {start,rozjezd,jizda,cihla,pocihle,cil};
stavy stav;       // Kde jsme na trati
int8 cas;         // Cas hrany bila/cerna v radce
int8 stred;       // Vystredeni kolecka
int16 odocounter; // Zaznamenani aktualniho stavu pocitadla odometrie
int16 last_log_odo; // Posledni stav odometrie poznamenany do logu
int16 last_log;   // Cislo posledniho zaznamu v logu v EEPROM
int8 bb_h[MAXLOG];  // Cerna skrinka MSB
int8 bb_l[MAXLOG];  // Cerna skrinka LSB
int8 log;         // Pocitadlo pro cernou skrinku
int16 bb;         // Posledni nactena udalost z Black Boxu
int16 timer_pom;  // Pomocna promenna pro atomicke nacteni hodnoty timeru1
int8 rr;          // Promenna na ulozeni Rozumne rychlost
int8 rrold;
 
// Zobrazeni jednoho byte na modulu LEDbar
inline void disp(int8 x)
{
   int n;
 
   for(n=0;n<=7;n++)
   {
      if (bit_test(x,0)) output_low(DATA); else output_high(DATA);
      output_high(CP);
      x>>=1;
      output_low(CP);
   }
}
 
// Blikani LEDbarem ve stilu Night Rider
void NightRider(int8 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(40);
      }
      for(n=0;n<7;n++)
      {
         disp(i);
         rotate_right(&i, 1);
         delay_ms(40);
      }
   }
   disp(i);
   delay_ms(40);
   i=0;
   disp(i);
}
 
// Zaznam LOGu do EEPROM
void SaveLog(int8 log)
{
   int8 n;
 
   for(n=0;n<=(log*2);n+=2)   // Ulozeni Black Boxu do EEPROM
   {
      write_eeprom(n,bb_h[n/2]);
      write_eeprom(n+1,bb_l[n/2]);
   };
   write_eeprom(EEMAX,log);  // Zapis poctu zaznamu na konec EEPROM
}
 
// Brzdeni motorama stridou 1:1
void brzda()
{
   int8 n,i;
 
   set_pwm1_duty(0);    // vypni PWM
   set_pwm2_duty(0);
   setup_ccp1(CCP_OFF);
   setup_ccp2(CCP_OFF);
   for (n=0;n<200;n++)
   {
      output_low(MOT_L);
      output_low(MOT_R);
      output_high(MOT_1);
      output_high(MOT_2);
      delay_us(200);
      output_high(MOT_L);
      output_high(MOT_R);
      output_low(MOT_1);
      output_low(MOT_2);
      delay_us(200);
   }
   output_low(MOT_L); // smer vpred
   output_low(MOT_R);
   setup_ccp1(CCP_PWM); // RC1               // Zapni PWM pro motory
   setup_ccp2(CCP_PWM); // RC2
}
 
void SetServo(int8 angle)
{
   int8 n;
 
   for(n=0; n<14; n++)
   {
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
      delay_us(1000);
      delay_us(stred);
      delay_us(stred);
      delay_us(stred);
      delay_us(angle);
      delay_us(angle);
      output_low(SERVO);
      delay_ms(18);
   }
}
 
inline void SetServoQ(int8 angle)
{
   output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
   delay_us(1000);
   delay_us(stred);
   delay_us(stred);
   delay_us(stred);
   delay_us(angle);
   delay_us(angle);
   output_low(SERVO);
}
 
//---------------------------- INT --------------------------------
#int_EXT
EXT_isr()   // Preruseni od prekazky
{
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vypni
   set_pwm2_duty(0);
   output_high(MOT_L);
   output_low(MOT_R);
   // Ujistime se, ze prijaty signal je z naseho IR vysilace
   output_high(IRTX);   // Vypni LED na detekci prekazky
   delay_ms(2);
   if (IRRX)    // stale nas signal?
   {
      output_low(MOT_L); // je odraz -> vpred
      output_low(MOT_R);
      return;
   };
   output_low(IRTX);    // Zapni LED na detekci prekazky
   delay_ms(10);
   if (!IRRX)     // stale nas signal?
   {
      output_low(MOT_L); // neni odraz -> vpred
      output_low(MOT_R);
      return;
   };
 
   timer_pom=get_timer1();       // Obsah Timeru1 se musi nacist atomicky
   bb_l[log]=make8(timer_pom,0); // Zaznam do logu
   bb_h[log]=make8(timer_pom,1)|0x80;  // Cihla ma nastaven nejvyssi bit
   if(log<MAXLOG) log++;        // Ukazatel na dalsi zaznam
//!!! Vyzkouset, ze nepreteka LOG
 
//!!!   if(stav==cihla) while(true); // Zastav na furt, konec drahy
//   if(stav==cihla) return; // Po druhe nic neobjizdej
// Pozor na rozjezd
 
   if(stav==jizda)   // Objed cihlu
   {
      #include ".\objizdka_L.c"
   };
   last_log_odo=get_timer1()+16;  // Pul metru po cihle nezaznamenavej do LOGu
}
 
 
//---------------------------------- MAIN --------------------------------------
void main()
{
   int8 offset;   // Promena pro ulozeni offsetu
   int8 r1;       // Rychlost motoru 1
   int8 r2;       // Rychlost motoru 2
 
   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_EXTERNAL);               // Cita pulzy z odometrie z praveho kola
   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(0b11111001);     // Nastaveni vstup/vystup pro branu C, protoze se to nedela automaticky
 
   set_pwm1_duty(0);    // Zastav motory
   set_pwm2_duty(0);
   output_low(MOT_L);   // Nastav smer vpred
   output_low(MOT_R);
 
   output_low(IRTX);   // Zapni LED na detekci prekazky
 
   NightRider(1);    // Aby se poznalo, ze byl RESET
                     // taky se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
 
   //... Nastaveni sonaru ...
   i2c_start();
   i2c_write(SONAR_ADR);
   i2c_write(0x02);  // dosah
   i2c_write(0x03);  // n*43mm
   i2c_stop();
   i2c_start();
   i2c_write(SONAR_ADR);
   i2c_write(0x01);  // zesileni
   i2c_write(0x01);  // male, pro eliminaci echa z minuleho mereni
   i2c_stop();
 
// pro ladeni sonaru
/*
   while(true)
   {
         i2c_start();     // Sonar Ping
      i2c_write(0xE0);
      i2c_write(0x0);
      i2c_write(0x51);  // 50 mereni v palcich, 51 mereni v cm, 52 v us
      i2c_stop();
      delay_ms(100);
      i2c_start();     // Odraz ze sonaru
      i2c_write(0xE0);
      i2c_write(0x3);
      i2c_stop();
      i2c_start();
      i2c_write(0xE1);
      cas=i2c_read(0);
      i2c_stop();
      disp(cas);
   }
*/
 
   //... Nastaveni kamery ...
   i2c_start();      // Soft RESET kamery
   i2c_write(CAMERA_ADR);        // 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(CAMERA_ADR);
   i2c_write(0x28);
   i2c_write(0b01000001);
   i2c_stop();
 
/*
   i2c_start();      // Contrast (nema podstatny vliv na obraz)
   i2c_write(0xC0);
   i2c_write(0x05);
   i2c_write(0xA0);  // 48h
   i2c_stop();
 
   i2c_start();      // Band Filter (pokud by byl problem se zarivkama 50Hz)
   i2c_write(0xC0);
   i2c_write(0x2D);
   i2c_write(0x04 | 0x03);
   i2c_stop();
*/
 
   i2c_start();      // Fame Rate
   i2c_write(CAMERA_ADR);
   i2c_write(0x2B);
   i2c_write(0x00);  // cca 17ms (puvodni hodnota 5Eh = 20ms)
   i2c_stop();
 
   i2c_start();      // VSTRT
   i2c_write(CAMERA_ADR);
   i2c_write(0x19);
   i2c_write(118);   // prostredni radka
   i2c_stop();
 
   i2c_start();      // VEND
   i2c_write(CAMERA_ADR);
   i2c_write(0x1A);
   i2c_write(118);
   i2c_stop();
 
   NightRider(1);    // Musi se dat cas kamere na AGC a AEC
 
   { // Mereni expozice
      int8 t1,t2;
 
      i2c_start();      // Brightness, zacni od uplne tmy
      i2c_write(CAMERA_ADR);
      i2c_write(0x06);
      i2c_write(0);  // 80h default
      i2c_stop();
      delay_ms(50);
 
      for(offset=0x04;offset<(255-0x04);offset+=0x04)   // Zacni od jasu 10h
      {
         i2c_start();      // Brightness
         i2c_write(CAMERA_ADR);
         i2c_write(0x06);
         i2c_write(offset);  // 80h default
         i2c_stop();
         disp(offset);
         delay_ms(50);
 
         t1=0;
         t2=0;
         while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
         delay_ms(5);
         while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
         set_timer0(0);          // Vynuluj pocitadlo casu
         if(!input(PIX)) continue;
         while(input(PIX));
         t1=get_timer0();    // Precti cas z citace casu hrany
         set_timer0(0);          // Vynuluj pocitadlo casu
         while(!input(PIX));
         t2=get_timer0();
 
         if((t1>60) && (t1<140) && (t2>5) && (t2<=10)) break; // Vidis, co mas?
 
         delay_ms(2);   // Preskoc druhou radku z kamery
      };
      delay_ms(1000);   // Nech chvili na displayi zmerenou hodnotu
   }
 
   set_adc_channel(CERVENA);   // --- Kroutitko pro jas ---
   delay_ms(1);
   offset=read_adc();
   offset &= 0b11111100;   // Dva nejnizsi bity ignoruj
//   offset += 0x70;         // Jas nebude nikdy nizsi
   disp(offset);
   i2c_start();            // Brightness
   i2c_write(CAMERA_adr);
   i2c_write(0x06);
   i2c_write(offset);  // 80h default
   i2c_stop();
   delay_ms(1000);   // Nech hodnotu chvili na displayi
 
   set_adc_channel(ZELENA);   // --- Kroutitko pro vykon motoru ---
   delay_ms(1);
   rr=read_adc()>>2; // 0-31  // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni !!!
   rrold=rr;
 
   cas=CASAVR-CASMIN;   // Inicializace promenych, aby neslo servo za roh
                        // a aby se to rozjelo jeste dneska
   stav=start;          // Jsme na startu
   set_timer1(0);       // Vynuluj citac odometrie
   log=0;               // Zacatek logu v cerne skrince
   last_log_odo=0;      // Posledni zaznam odometrie do logu
 
   if(BUMPER)  // Kdyz nekdo na zacatku drzi naraznik, vymaz log
   {
      bb_l[0]=0;        // Zapis na pozici 0 vzdalenost 0
      bb_h[0]=0;
      SaveLog(0);       // Zapis do EEPROM pocet zaznamu 0, tedy jeden zaznam
      disp(0x55);       // Blikni pro potvrzeni
      delay_ms(200);
      disp(0xAA);
      delay_ms(200);
   }
 
   last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano?
   bb=MAKE16(read_eeprom(1) & 0x7F,read_eeprom(0)); // Precti prvni zaznam
 
   // ........................... Hlavni smycka ................................
   while(true)
   {
      int8 pom;
      int8 n;
 
next_snap:
 
      pom=0;
      disable_interrupts(GLOBAL);  //----------------------- Critical
      while(input(HREF));     // Preskoc 1. radku
      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z 2. 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 ?
         if(!input(PIX))   // Pokud se X-krat za sebou precetla CERNA
         if(!input(PIX))
//         if(!input(PIX))
         {
            pom=get_timer0();    // Precti cas z citace casu hrany
            break;
         };
      };
      while(input(HREF));      // Pockej na shozeni signalu HREF
 
      if((pom<CASMAX) && (pom>CASMIN)) cas=pom;  // Orizni konce radku
      // Na konci obrazovaho radku to blbne. Jednak chyba od apertury
      // a vubec to nejak na kraji nefunguje.
 
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
      delay_us(1000);
      delay_us(stred);
      delay_us(stred);
      delay_us(stred);
      delay_us(cas);
      delay_us(cas);
      output_low(SERVO);
 
      if(pom==0) // Kamera nevidi caru, poznamenej to do logu
      {
         if(last_log_odo>get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu
         {
            timer_pom=get_timer1();          // Timer se musi poznamenat atomicky
            bb_l[log]=make8(timer_pom,0); // Zaznam
            bb_h[log]=make8(timer_pom,1);
            last_log_odo=timer_pom+8;     // Dalsi mereni nejdrive po ujeti 24 cm
            if(log<MAXLOG) log++;         // Ukazatel na dalsi zaznam
         }
      }
 
      // Elektronicky diferencial
      if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
      if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
 
      enable_interrupts(GLOBAL);    //----------------------- Critical
 
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
 
//      r1<<=1;     // Rychlost je dvojnasobna
//      r2<<=1;     // Rozsah 2 az 184
 
/* Nerozumna rychlost po cihle
      if ((stav==cihla)&&(get_timer1()>(odocounter+5))) // Snizime rychlost po ujeti
      {
         rr=rrold;
         stav=pocihle;
      };
*/
      if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle))      // Jizda
      {
         set_pwm1_duty(r1);
         set_pwm2_duty(r2);
      }
      else
      {
         set_pwm1_duty(0);       // Zastaveni
         set_pwm2_duty(0);
      };
 
      if((stav==rozjezd)&&(get_timer1()>10)) // musi ujet alespon 31cm
      {
         ext_int_edge(H_TO_L);         // Nastav podminky preruseni od cihly
         INT0IF=0;                     // Zruseni predesle udalosti od startera
         enable_interrupts(INT_EXT);
         enable_interrupts(GLOBAL);
         stav=jizda;
      };
 
      if(stav==start)         // Snimkuje, toci servem a ceka na start
      {
         set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
         Delay_ms(1);
         stred=read_adc();
         if(!input(PROXIMITY))
         {
            disp(0x80);
            while(input(PROXIMITY)); // Cekej, dokud starter neda ruku pryc
            set_timer1(0);         // Vynuluj citac odometrie
            set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
            set_pwm2_duty(255);
            disp(0x01);
            while(get_timer1()<=4) // Ujed alespon 12cm
            {
               set_adc_channel(LMAX);    // Levy UV sensor
               delay_us(40);
               if(read_adc()<THR) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
               set_adc_channel(RMAX);    // Pravy UV sensor
               delay_us(40);
               if(read_adc()<THR) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
               cas=CASAVR-CASMIN;    // Cara je rovne
            };
            stav=rozjezd;
         };
      }
 
      pom=0x80;    // Zobrazeni pozice cary na displayi
      for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
      disp(pom);
 
      while(true) // Ve zbytku casu snimku cti krajni UV senzory a naraznik
      {
         if(BUMPER) // Sakra, do neceho jsme narazili a nevideli jsme to!
         {
            set_pwm1_duty(0);    // couvni, rovne dozadu
            set_pwm2_duty(0);
            output_high(MOT_L);
            output_high(MOT_R);
            disp(0x5A);
            SetServo(CASAVR-CASMIN);
            SaveLog(log-1);      // Zapis Black Boxu do EEPROM
            output_low(MOT_L);   // Vypni motory
            output_low(MOT_R);
         };
         set_adc_channel(LMAX);    // Levy UV sensor
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
         if(read_adc()<THR) cas=CASMIN;
         set_adc_channel(RMAX);    // Pravy UV sensor
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
         if(read_adc()<THR) cas=CASMAX;
      };
   }
}

Rev 234	Rev 236
1	`//******* Robot Camerus pro IstRobot 2007 **********`	1	`//******* Robot Camerus pro IstRobot 2007 **********`
2	`//"$Id: camerus.c 234 2007-04-16 21:19:54Z kaklik $"`	2	`//"$Id: camerus.c 236 2007-04-17 14:40:34Z kakl $"`
3	`//*****************************************************`	3	`//*****************************************************`
4		4
5	`#include ".\camerus.h"`	5	`#include ".\camerus.h"`
6		6
7	`#USE FAST_IO (C) // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery`	7	`#USE FAST_IO (C) // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery`
8		8
-		9	`// Rychlostni konstanty`
-		10	`#define RR_CIHLA 20 // Rozumna rychlost pro objizdeni cihly`
-		11	`#define RR_PRERUSENI 20 // Rozumna rychlost pro priblizeni se k preruseni`
-		12	`#define BRZDNA_DRAHA 8 // Jak daleko pred problemem se zacne brzdit`
-		13
9	`#define CAMERA_adr 0xC0`	14	`#define CAMERA_ADR 0xC0`
10	`#define SONAR_adr 0xE0`	15	`#define SONAR_ADR 0xE0`
11		16
12	`// A/D vstupy`	17	`// A/D vstupy`
13	`#define RMAX 4 // AN4/RA5 - leve cidlo na vyjeti z cary`	18	`#define RMAX 4 // AN4/RA5 - leve cidlo na vyjeti z cary`
14	`#define LMAX 3 // AN3/RA3 - prave cidlo na vyjeti z cary`	19	`#define LMAX 3 // AN3/RA3 - prave cidlo na vyjeti z cary`
15	`#define CERVENA 2 // AN2/RA2 - cervene kroutitko`	20	`#define CERVENA 2 // AN2/RA2 - cervene kroutitko`
16	`#define ZELENA 1 // AN1/RA0 - zelene kroutitko`	21	`#define ZELENA 1 // AN1/RA0 - zelene kroutitko`
17	`#define MODRA 0 // AN0/RA1 - modre kroutitko`	22	`#define MODRA 0 // AN0/RA1 - modre kroutitko`
18		23
19	`// I/O`	24	`// I/O`
20	`#define HREF PIN_C5 // Signal HREF z kamery (v H po celou dobu radku)`	25	`#define HREF PIN_C5 // Signal HREF z kamery (v H po celou dobu radku)`
21	`#define PIX PIN_C6 // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))`	26	`#define PIX PIN_C6 // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))`
22	`#define SERVO PIN_B4 // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))`	27	`#define SERVO PIN_B4 // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))`
23	`#define MOT_L PIN_B5 // Smer otaceni leveho motoru; druhy pol je RC2`	28	`#define MOT_L PIN_B5 // Smer otaceni leveho motoru; druhy pol je RC2`
24	`#define MOT_R PIN_B6 // Smer otaceni praveho motoru; druhy pol je RC1`	29	`#define MOT_R PIN_B6 // Smer otaceni praveho motoru; druhy pol je RC1`
25	`#define MOT_1 PIN_C1 // PWM vystpy motoru`	30	`#define MOT_1 PIN_C1 // PWM vystpy motoru`
26	`#define MOT_2 PIN_C2 //`	31	`#define MOT_2 PIN_C2 //`
27	`#define DATA PIN_B2 // K modulu LEDbar data`	32	`#define DATA PIN_B2 // K modulu LEDbar data`
28	`#define CP PIN_B1 // K modulu LEDbar hodiny`	33	`#define CP PIN_B1 // K modulu LEDbar hodiny`
29	`//#define ODO PIN_C0 // Ze snimace z odometrie z praveho kola na TIMER1`	34	`//#define ODO PIN_C0 // Ze snimace z odometrie z praveho kola na TIMER1`
30	`// Jeden impuls je 31,25mm`	35	`// Jeden impuls je 31,25mm`
31	`#define IRRX PIN_B0 // Vstup INT, generuje preruseni pri prekazce`	36	`#define IRRX !input(PIN_B0) // Vstup INT, generuje preruseni pri prekazce`
32	`#define IRTX PIN_B3 // Modulovani vysilaci IR LED na detekci prekazky`	37	`#define IRTX PIN_B3 // Modulovani vysilaci IR LED na detekci prekazky`
33	`#define PROXIMITY PIN_C7 // Cidlo kratkeho dosahu na cihlu`	38	`#define PROXIMITY PIN_C7 // Cidlo kratkeho dosahu na cihlu`
-		39	`#define BUMPER !input(PIN_A4) // Naraznik`
34		40
35	`#define CASMIN 6 // Rozsah radku snimace`	41	`#define CASMIN 6 // Rozsah radku snimace`
36	`#define CASMAX 192`	42	`#define CASMAX 192`
37	`#define CASAVR ((CASMAX+CASMIN) / 2)`	43	`#define CASAVR ((CASMAX+CASMIN) / 2)`
38		44
-		45	`#define EEMAX 255 // Konec EEPROM`
-		46	`#define MAXLOG 0x30 // Maximalni pocet zaznamu v logu`
-		47	`#if MAXLOG>(EEMAX/2-1)`
-		48	`#error Prekrocena velikost EEPROM`
-		49	`#endif`
-		50
39	`#define OFFSETO 0x9F //0x9F // Vystredeni serva pro objeti prekazky`	51	`#define OFFSETO 0x9F //0x9F // Vystredeni serva pro objeti prekazky`
40		52
41	`#define THR 90 // Threshold pro UV cidla na caru`	53	`#define THR 90 // Threshold pro UV cidla na caru`
42		54
43	`#byte INTCON = 0x0B // Interrupt configuration register`	55	`#byte INTCON = 0x0B // Interrupt configuration register`
44	`#bit GIE = INTCON.7`	56	`#bit GIE = INTCON.7`
45	`#bit PEIE = INTCON.6`	57	`#bit PEIE = INTCON.6`
46	`#bit TMR0IE = INTCON.5`	58	`#bit TMR0IE = INTCON.5`
47	`#bit INT0IE = INTCON.4`	59	`#bit INT0IE = INTCON.4`
48	`#bit RBIE = INTCON.3`	60	`#bit RBIE = INTCON.3`
49	`#bit TMR0IF = INTCON.2`	61	`#bit TMR0IF = INTCON.2`
50	`#bit INT0IF = INTCON.1`	62	`#bit INT0IF = INTCON.1`
51	`#bit RBIF = INTCON.0`	63	`#bit RBIF = INTCON.0`
52		64
53	`enum stavy {start,rozjezd,jizda,cihla,pocihle,cil};`	65	`enum stavy {start,rozjezd,jizda,cihla,pocihle,cil};`
54	`stavy stav; // Kde jsme na trati`	66	`stavy stav; // Kde jsme na trati`
55	`int8 cas; // Cas hrany bila/cerna v radce`	67	`int8 cas; // Cas hrany bila/cerna v radce`
56	`int8 stred; // Vystredeni kolecka`	68	`int8 stred; // Vystredeni kolecka`
57	`int16 odocounter; // Zaznamenani aktualniho stavu pocitadla odometrie`	69	`int16 odocounter; // Zaznamenani aktualniho stavu pocitadla odometrie`
-		70	`int16 last_log_odo; // Posledni stav odometrie poznamenany do logu`
-		71	`int16 last_log; // Cislo posledniho zaznamu v logu v EEPROM`
-		72	`int8 bb_h[MAXLOG]; // Cerna skrinka MSB`
-		73	`int8 bb_l[MAXLOG]; // Cerna skrinka LSB`
-		74	`int8 log; // Pocitadlo pro cernou skrinku`
-		75	`int16 bb; // Posledni nactena udalost z Black Boxu`
-		76	`int16 timer_pom; // Pomocna promenna pro atomicke nacteni hodnoty timeru1`
58	`int8 rr; // Promenna na ulozeni Rozumne rychlost`	77	`int8 rr; // Promenna na ulozeni Rozumne rychlost`
59	`int8 rrold;`	78	`int8 rrold;`
60		79
61	`// Zobrazeni jednoho byte na modulu LEDbar`	80	`// Zobrazeni jednoho byte na modulu LEDbar`
62	`inline void disp(int8 x)`	81	`inline void disp(int8 x)`
63	`{`	82	`{`
64	`int n;`	83	`int n;`
65		84
66	`for(n=0;n<=7;n++)`	85	`for(n=0;n<=7;n++)`
67	`{`	86	`{`
68	`if (x & 1 == 1) output_low(DATA); else output_high(DATA);`	87	`if (bit_test(x,0)) output_low(DATA); else output_high(DATA);`
69	`output_high(CP);`	88	`output_high(CP);`
70	`x>>=1;`	89	`x>>=1;`
71	`output_low(CP);`	90	`output_low(CP);`
72	`}`	91	`}`
73	`}`	92	`}`
74		93
75	`// Blikani LEDbarem ve stilu Night Rider`	94	`// Blikani LEDbarem ve stilu Night Rider`
76	`void NightRider(int8 x)`	95	`void NightRider(int8 x)`
77	`{`	96	`{`
78	`int n,i,j;`	97	`int n,i,j;`
79		98
80	`for(j=0;j<x;j++)`	99	`for(j=0;j<x;j++)`
81	`{`	100	`{`
82	`i=0x01;`	101	`i=0x01;`
83	`for(n=0;n<7;n++)`	102	`for(n=0;n<7;n++)`
84	`{`	103	`{`
85	`disp(i);`	104	`disp(i);`
86	`rotate_left(&i, 1);`	105	`rotate_left(&i, 1);`
87	`delay_ms(40);`	106	`delay_ms(40);`
88	`}`	107	`}`
89	`for(n=0;n<7;n++)`	108	`for(n=0;n<7;n++)`
90	`{`	109	`{`
91	`disp(i);`	110	`disp(i);`
92	`rotate_right(&i, 1);`	111	`rotate_right(&i, 1);`
93	`delay_ms(40);`	112	`delay_ms(40);`
94	`}`	113	`}`
95	`}`	114	`}`
96	`disp(i);`	115	`disp(i);`
97	`delay_ms(40);`	116	`delay_ms(40);`
98	`i=0;`	117	`i=0;`
99	`disp(i);`	118	`disp(i);`
100	`}`	119	`}`
101		120
-		121	`// Zaznam LOGu do EEPROM`
-		122	`void SaveLog(int8 log)`
-		123	`{`
-		124	`int8 n;`
-		125
-		126	`for(n=0;n<=(log*2);n+=2) // Ulozeni Black Boxu do EEPROM`
-		127	`{`
-		128	`write_eeprom(n,bb_h[n/2]);`
-		129	`write_eeprom(n+1,bb_l[n/2]);`
-		130	`};`
-		131	`write_eeprom(EEMAX,log); // Zapis poctu zaznamu na konec EEPROM`
-		132	`}`
-		133
102	`// Brzdeni motorama stridou 1:1`	134	`// Brzdeni motorama stridou 1:1`
103	`void brzda()`	135	`void brzda()`
104	`{`	136	`{`
105	`int8 n,i;`	137	`int8 n,i;`
106		138
107	`set_pwm1_duty(0); // vypni PWM`	139	`set_pwm1_duty(0); // vypni PWM`
108	`set_pwm2_duty(0);`	140	`set_pwm2_duty(0);`
109	`setup_ccp1(CCP_OFF);`	141	`setup_ccp1(CCP_OFF);`
110	`setup_ccp2(CCP_OFF);`	142	`setup_ccp2(CCP_OFF);`
111	`for (n=0;n<200;n++)`	143	`for (n=0;n<200;n++)`
112	`{`	144	`{`
113	`output_low(MOT_L);`	145	`output_low(MOT_L);`
114	`output_low(MOT_R);`	146	`output_low(MOT_R);`
115	`output_high(MOT_1);`	147	`output_high(MOT_1);`
116	`output_high(MOT_2);`	148	`output_high(MOT_2);`
117	`delay_us(200);`	149	`delay_us(200);`
118	`output_high(MOT_L);`	150	`output_high(MOT_L);`
119	`output_high(MOT_R);`	151	`output_high(MOT_R);`
120	`output_low(MOT_1);`	152	`output_low(MOT_1);`
121	`output_low(MOT_2);`	153	`output_low(MOT_2);`
122	`delay_us(200);`	154	`delay_us(200);`
123	`}`	155	`}`
124	`output_low(MOT_L); // smer vpred`	156	`output_low(MOT_L); // smer vpred`
125	`output_low(MOT_R);`	157	`output_low(MOT_R);`
126	`setup_ccp1(CCP_PWM); // RC1 // Zapni PWM pro motory`	158	`setup_ccp1(CCP_PWM); // RC1 // Zapni PWM pro motory`
127	`setup_ccp2(CCP_PWM); // RC2`	159	`setup_ccp2(CCP_PWM); // RC2`
128	`}`	160	`}`
129		161
130	`void SetServo(int8 angle)`	162	`void SetServo(int8 angle)`
131	`{`	163	`{`
132	`int8 n;`	164	`int8 n;`
133		165
134	`for(n=0; n<14; n++)`	166	`for(n=0; n<14; n++)`
135	`{`	167	`{`
136	`output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo`	168	`output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo`
137	`delay_us(1000);`	169	`delay_us(1000);`
138	`delay_us(stred);`	170	`delay_us(stred);`
139	`delay_us(stred);`	171	`delay_us(stred);`
140	`delay_us(stred);`	172	`delay_us(stred);`
141	`delay_us(angle);`	173	`delay_us(angle);`
142	`delay_us(angle);`	174	`delay_us(angle);`
143	`output_low(SERVO);`	175	`output_low(SERVO);`
144	`delay_ms(18);`	176	`delay_ms(18);`
145	`}`	177	`}`
146	`}`	178	`}`
147		179
148	`inline void SetServoQ(int8 angle)`	180	`inline void SetServoQ(int8 angle)`
149	`{`	181	`{`
150	`output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo`	182	`output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo`
151	`delay_us(1000);`	183	`delay_us(1000);`
152	`delay_us(stred);`	184	`delay_us(stred);`
153	`delay_us(stred);`	185	`delay_us(stred);`
154	`delay_us(stred);`	186	`delay_us(stred);`
155	`delay_us(angle);`	187	`delay_us(angle);`
156	`delay_us(angle);`	188	`delay_us(angle);`
157	`output_low(SERVO);`	189	`output_low(SERVO);`
158	`}`	190	`}`
159		191
160	`//---------------------------- INT --------------------------------`	192	`//---------------------------- INT --------------------------------`
161	`#int_EXT`	193	`#int_EXT`
162	`EXT_isr() // Preruseni od prekazky`	194	`EXT_isr() // Preruseni od prekazky`
163	`{`	195	`{`
164	`set_pwm1_duty(0); // zabrzdi levym kolem, prave vypni`	196	`set_pwm1_duty(0); // zabrzdi levym kolem, prave vypni`
165	`set_pwm2_duty(0);`	197	`set_pwm2_duty(0);`
166	`output_high(MOT_L);`	198	`output_high(MOT_L);`
167	`output_low(MOT_R);`	199	`output_low(MOT_R);`
168	`// Ujistime se, ze prijaty signal je z naseho IR vysilace`	200	`// Ujistime se, ze prijaty signal je z naseho IR vysilace`
169	`output_high(IRTX); // Vypni LED na detekci prekazky`	201	`output_high(IRTX); // Vypni LED na detekci prekazky`
170	`delay_ms(2);`	202	`delay_ms(2);`
171	`if (!input(IRRX)) // stale nas signal?`	203	`if (IRRX) // stale nas signal?`
172	`{`	204	`{`
173	`output_low(MOT_L); // je odraz -> vpred`	205	`output_low(MOT_L); // je odraz -> vpred`
174	`output_low(MOT_R);`	206	`output_low(MOT_R);`
175	`return;`	207	`return;`
176	`};`	208	`};`
177	`output_low(IRTX); // Zapni LED na detekci prekazky`	209	`output_low(IRTX); // Zapni LED na detekci prekazky`
178	`delay_ms(10);`	210	`delay_ms(10);`
179	`if (input(IRRX)) // stale nas signal?`	211	`if (!IRRX) // stale nas signal?`
180	`{`	212	`{`
181	`output_low(MOT_L); // neni odraz -> vpred`	213	`output_low(MOT_L); // neni odraz -> vpred`
182	`output_low(MOT_R);`	214	`output_low(MOT_R);`
183	`return;`	215	`return;`
184	`};`	216	`};`
185		217
-		218	`timer_pom=get_timer1(); // Obsah Timeru1 se musi nacist atomicky`
-		219	`bb_l[log]=make8(timer_pom,0); // Zaznam do logu`
-		220	`bb_h[log]=make8(timer_pom,1)\|0x80; // Cihla ma nastaven nejvyssi bit`
-		221	`if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam`
-		222	`//!!! Vyzkouset, ze nepreteka LOG`
-		223
186	`//!!! if(stav==cihla) while(true); // Zastav na furt, konec drahy`	224	`//!!! if(stav==cihla) while(true); // Zastav na furt, konec drahy`
187	`// if(stav==cihla) return; // Po druhe nic neobjizdej`	225	`// if(stav==cihla) return; // Po druhe nic neobjizdej`
188	`// Pozor na rozjezd`	226	`// Pozor na rozjezd`
189		227
190	`if(stav==jizda) // Objed cihlu`	228	`if(stav==jizda) // Objed cihlu`
191	`{`	229	`{`
192	`#include ".\objizdka_L.c"`	230	`#include ".\objizdka_L.c"`
193	`}`	231	`};`
-		232	`last_log_odo=get_timer1()+16; // Pul metru po cihle nezaznamenavej do LOGu`
194	`}`	233	`}`
195		234
196		235
197	`//---------------------------------- MAIN --------------------------------------`	236	`//---------------------------------- MAIN --------------------------------------`
198	`void main()`	237	`void main()`
199	`{`	238	`{`
200	`int8 offset; // Promena pro ulozeni offsetu`	239	`int8 offset; // Promena pro ulozeni offsetu`
201	`int8 r1; // Rychlost motoru 1`	240	`int8 r1; // Rychlost motoru 1`
202	`int8 r2; // Rychlost motoru 2`	241	`int8 r2; // Rychlost motoru 2`
203		242
204	`setup_adc_ports(ALL_ANALOG); // Zapnuti A/D prevodniku pro cteni kroutitek`	243	`setup_adc_ports(ALL_ANALOG); // Zapnuti A/D prevodniku pro cteni kroutitek`
205	`setup_adc(ADC_CLOCK_INTERNAL);`	244	`setup_adc(ADC_CLOCK_INTERNAL);`
206	`setup_timer_0(RTCC_INTERNAL\|RTCC_DIV_1); // Casovac pro mereni casu hrany W/B v radce`	245	`setup_timer_0(RTCC_INTERNAL\|RTCC_DIV_1); // Casovac pro mereni casu hrany W/B v radce`
207	`setup_timer_1(T1_EXTERNAL); // Cita pulzy z odometrie z praveho kola`	246	`setup_timer_1(T1_EXTERNAL); // Cita pulzy z odometrie z praveho kola`
208	`setup_timer_2(T2_DIV_BY_16,255,1); // Casovac PWM motoru`	247	`setup_timer_2(T2_DIV_BY_16,255,1); // Casovac PWM motoru`
209	`setup_ccp1(CCP_PWM); // RC1 // PWM pro motory`	248	`setup_ccp1(CCP_PWM); // RC1 // PWM pro motory`
210	`setup_ccp2(CCP_PWM); // RC2`	249	`setup_ccp2(CCP_PWM); // RC2`
211	`setup_comparator(NC_NC_NC_NC);`	250	`setup_comparator(NC_NC_NC_NC);`
212	`setup_vref(FALSE);`	251	`setup_vref(FALSE);`
213		252
214	`set_tris_c(0b11111001); // Nastaveni vstup/vystup pro branu C, protoze se to nedela automaticky`	253	`set_tris_c(0b11111001); // Nastaveni vstup/vystup pro branu C, protoze se to nedela automaticky`
215		254
216	`set_pwm1_duty(0); // Zastav motory`	255	`set_pwm1_duty(0); // Zastav motory`
217	`set_pwm2_duty(0);`	256	`set_pwm2_duty(0);`
218	`output_low(MOT_L); // Nastav smer vpred`	257	`output_low(MOT_L); // Nastav smer vpred`
219	`output_low(MOT_R);`	258	`output_low(MOT_R);`
220		259
221	`output_low(IRTX); // Zapni LED na detekci prekazky`	260	`output_low(IRTX); // Zapni LED na detekci prekazky`
222		261
223	`NightRider(1); // Aby se poznalo, ze byl RESET`	262	`NightRider(1); // Aby se poznalo, ze byl RESET`
224	`// taky se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy`	263	`// taky se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy`
225		264
226	`//... Nastaveni sonaru ...`	265	`//... Nastaveni sonaru ...`
227	`i2c_start();`	266	`i2c_start();`
228	`i2c_write(SONAR_adr);`	267	`i2c_write(SONAR_ADR);`
229	`i2c_write(0x02); // dosah`	268	`i2c_write(0x02); // dosah`
230	`i2c_write(0x03); // n*43mm`	269	`i2c_write(0x03); // n*43mm`
231	`i2c_stop();`	270	`i2c_stop();`
232	`i2c_start();`	271	`i2c_start();`
233	`i2c_write(SONAR_adr);`	272	`i2c_write(SONAR_ADR);`
234	`i2c_write(0x01); // zesileni`	273	`i2c_write(0x01); // zesileni`
235	`i2c_write(0x01); // male, pro eliminaci echa z minuleho mereni`	274	`i2c_write(0x01); // male, pro eliminaci echa z minuleho mereni`
236	`i2c_stop();`	275	`i2c_stop();`
237		276
238	`// pro ladeni sonaru`	277	`// pro ladeni sonaru`
239	`/*`	278	`/*`
240	`while(true)`	279	`while(true)`
241	`{`	280	`{`
242	`i2c_start(); // Sonar Ping`	281	`i2c_start(); // Sonar Ping`
243	`i2c_write(0xE0);`	282	`i2c_write(0xE0);`
244	`i2c_write(0x0);`	283	`i2c_write(0x0);`
245	`i2c_write(0x51); // 50 mereni v palcich, 51 mereni v cm, 52 v us`	284	`i2c_write(0x51); // 50 mereni v palcich, 51 mereni v cm, 52 v us`
246	`i2c_stop();`	285	`i2c_stop();`
247	`delay_ms(100);`	286	`delay_ms(100);`
248	`i2c_start(); // Odraz ze sonaru`	287	`i2c_start(); // Odraz ze sonaru`
249	`i2c_write(0xE0);`	288	`i2c_write(0xE0);`
250	`i2c_write(0x3);`	289	`i2c_write(0x3);`
251	`i2c_stop();`	290	`i2c_stop();`
252	`i2c_start();`	291	`i2c_start();`
253	`i2c_write(0xE1);`	292	`i2c_write(0xE1);`
254	`cas=i2c_read(0);`	293	`cas=i2c_read(0);`
255	`i2c_stop();`	294	`i2c_stop();`
256	`disp(cas);`	295	`disp(cas);`
257	`}`	296	`}`
258	`*/`	297	`*/`
259		298
260	`//... Nastaveni kamery ...`	299	`//... Nastaveni kamery ...`
261	`i2c_start(); // Soft RESET kamery`	300	`i2c_start(); // Soft RESET kamery`
262	`i2c_write(CAMERA_adr); // Pro single slave musi mit vsechny zapisy adresu C0h`	301	`i2c_write(CAMERA_ADR); // Pro single slave musi mit vsechny zapisy adresu C0h`
263	`i2c_write(0x12); // Adresa registru COMH`	302	`i2c_write(0x12); // Adresa registru COMH`
264	`i2c_write(0x80 \| 0x24); // Zapis ridiciho slova`	303	`i2c_write(0x80 \| 0x24); // Zapis ridiciho slova`
265	`i2c_stop();`	304	`i2c_stop();`
266		305
267	`i2c_start(); // BW`	306	`i2c_start(); // BW`
268	`i2c_write(CAMERA_adr);`	307	`i2c_write(CAMERA_ADR);`
269	`i2c_write(0x28);`	308	`i2c_write(0x28);`
270	`i2c_write(0b01000001);`	309	`i2c_write(0b01000001);`
271	`i2c_stop();`	310	`i2c_stop();`
272		311
273	`/*`	312	`/*`
274	`i2c_start(); // Contrast (nema podstatny vliv na obraz)`	313	`i2c_start(); // Contrast (nema podstatny vliv na obraz)`
275	`i2c_write(0xC0);`	314	`i2c_write(0xC0);`
276	`i2c_write(0x05);`	315	`i2c_write(0x05);`
277	`i2c_write(0xA0); // 48h`	316	`i2c_write(0xA0); // 48h`
278	`i2c_stop();`	317	`i2c_stop();`
279		318
280	`i2c_start(); // Band Filter (pokud by byl problem se zarivkama 50Hz)`	319	`i2c_start(); // Band Filter (pokud by byl problem se zarivkama 50Hz)`
281	`i2c_write(0xC0);`	320	`i2c_write(0xC0);`
282	`i2c_write(0x2D);`	321	`i2c_write(0x2D);`
283	`i2c_write(0x04 \| 0x03);`	322	`i2c_write(0x04 \| 0x03);`
284	`i2c_stop();`	323	`i2c_stop();`
285	`*/`	324	`*/`
286		325
287	`i2c_start(); // Fame Rate`	326	`i2c_start(); // Fame Rate`
288	`i2c_write(CAMERA_adr);`	327	`i2c_write(CAMERA_ADR);`
289	`i2c_write(0x2B);`	328	`i2c_write(0x2B);`
290	`i2c_write(0x00); // cca 17ms (puvodni hodnota 5Eh = 20ms)`	329	`i2c_write(0x00); // cca 17ms (puvodni hodnota 5Eh = 20ms)`
291	`i2c_stop();`	330	`i2c_stop();`
292		331
293	`i2c_start(); // VSTRT`	332	`i2c_start(); // VSTRT`
294	`i2c_write(CAMERA_adr);`	333	`i2c_write(CAMERA_ADR);`
295	`i2c_write(0x19);`	334	`i2c_write(0x19);`
296	`i2c_write(118); // prostredni radka`	335	`i2c_write(118); // prostredni radka`
297	`i2c_stop();`	336	`i2c_stop();`
298		337
299	`i2c_start(); // VEND`	338	`i2c_start(); // VEND`
300	`i2c_write(CAMERA_adr);`	339	`i2c_write(CAMERA_ADR);`
301	`i2c_write(0x1A);`	340	`i2c_write(0x1A);`
302	`i2c_write(118);`	341	`i2c_write(118);`
303	`i2c_stop();`	342	`i2c_stop();`
304		343
305	`NightRider(1); // Musi se dat cas kamere na AGC a AEC`	344	`NightRider(1); // Musi se dat cas kamere na AGC a AEC`
306		345
307	`{ // Mereni expozice`	346	`{ // Mereni expozice`
308	`int8 t1,t2;`	347	`int8 t1,t2;`
309		348
310	`i2c_start(); // Brightness, zacni od uplne tmy`	349	`i2c_start(); // Brightness, zacni od uplne tmy`
311	`i2c_write(CAMERA_adr);`	350	`i2c_write(CAMERA_ADR);`
312	`i2c_write(0x06);`	351	`i2c_write(0x06);`
313	`i2c_write(0); // 80h default`	352	`i2c_write(0); // 80h default`
314	`i2c_stop();`	353	`i2c_stop();`
315	`delay_ms(50);`	354	`delay_ms(50);`
316		355
317	`for(offset=0x04;offset<(255-0x04);offset+=0x04) // Zacni od jasu 10h`	356	`for(offset=0x04;offset<(255-0x04);offset+=0x04) // Zacni od jasu 10h`
318	`{`	357	`{`
319	`i2c_start(); // Brightness`	358	`i2c_start(); // Brightness`
320	`i2c_write(CAMERA_adr);`	359	`i2c_write(CAMERA_ADR);`
321	`i2c_write(0x06);`	360	`i2c_write(0x06);`
322	`i2c_write(offset); // 80h default`	361	`i2c_write(offset); // 80h default`
323	`i2c_stop();`	362	`i2c_stop();`
324	`disp(offset);`	363	`disp(offset);`
325	`delay_ms(50);`	364	`delay_ms(50);`
326		365
327	`t1=0;`	366	`t1=0;`
328	`t2=0;`	367	`t2=0;`
329	`while(!input(HREF)); // Cekej nez se zacnou posilat pixely z radky`	368	`while(!input(HREF)); // Cekej nez se zacnou posilat pixely z radky`
330	`delay_ms(5);`	369	`delay_ms(5);`
331	`while(!input(HREF)); // Cekej nez se zacnou posilat pixely z radky`	370	`while(!input(HREF)); // Cekej nez se zacnou posilat pixely z radky`
332	`set_timer0(0); // Vynuluj pocitadlo casu`	371	`set_timer0(0); // Vynuluj pocitadlo casu`
333	`if(!input(PIX)) continue;`	372	`if(!input(PIX)) continue;`
334	`while(input(PIX));`	373	`while(input(PIX));`
335	`t1=get_timer0(); // Precti cas z citace casu hrany`	374	`t1=get_timer0(); // Precti cas z citace casu hrany`
336	`set_timer0(0); // Vynuluj pocitadlo casu`	375	`set_timer0(0); // Vynuluj pocitadlo casu`
337	`while(!input(PIX));`	376	`while(!input(PIX));`
338	`t2=get_timer0();`	377	`t2=get_timer0();`
339		378
340	`if((t1>60) && (t1<140) && (t2>5) && (t2<=10)) break; // Vidis, co mas?`	379	`if((t1>60) && (t1<140) && (t2>5) && (t2<=10)) break; // Vidis, co mas?`
341		380
342	`delay_ms(2); // Preskoc druhou radku z kamery`	381	`delay_ms(2); // Preskoc druhou radku z kamery`
343	`};`	382	`};`
344	`delay_ms(1000); // Nech chvili na displayi zmerenou hodnotu`	383	`delay_ms(1000); // Nech chvili na displayi zmerenou hodnotu`
345	`}`	384	`}`
346		385
347	`set_adc_channel(CERVENA); // --- Kroutitko pro jas ---`	386	`set_adc_channel(CERVENA); // --- Kroutitko pro jas ---`
348	`delay_ms(1);`	387	`delay_ms(1);`
349	`offset=read_adc();`	388	`offset=read_adc();`
350	`offset &= 0b11111100; // Dva nejnizsi bity ignoruj`	389	`offset &= 0b11111100; // Dva nejnizsi bity ignoruj`
351	`// offset += 0x70; // Jas nebude nikdy nizsi`	390	`// offset += 0x70; // Jas nebude nikdy nizsi`
352	`disp(offset);`	391	`disp(offset);`
353	`i2c_start(); // Brightness`	392	`i2c_start(); // Brightness`
354	`i2c_write(CAMERA_adr);`	393	`i2c_write(CAMERA_adr);`
355	`i2c_write(0x06);`	394	`i2c_write(0x06);`
356	`i2c_write(offset); // 80h default`	395	`i2c_write(offset); // 80h default`
357	`i2c_stop();`	396	`i2c_stop();`
358	`delay_ms(1000); // Nech hodnotu chvili na displayi`	397	`delay_ms(1000); // Nech hodnotu chvili na displayi`
359		398
360	`set_adc_channel(ZELENA); // --- Kroutitko pro vykon motoru ---`	399	`set_adc_channel(ZELENA); // --- Kroutitko pro vykon motoru ---`
361	`delay_ms(1);`	400	`delay_ms(1);`
362	`rr=read_adc()>>2; // 0-31 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni !!!`	401	`rr=read_adc()>>2; // 0-31 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni !!!`
363	`rrold=rr;`	402	`rrold=rr;`
364		403
365	`cas=CASAVR-CASMIN; // Inicializace promenych, aby neslo servo za roh`	404	`cas=CASAVR-CASMIN; // Inicializace promenych, aby neslo servo za roh`
366	`// a aby se to rozjelo jeste dneska`	405	`// a aby se to rozjelo jeste dneska`
367	`stav=start; // Jsme na startu`	406	`stav=start; // Jsme na startu`
368	`set_timer1(0); // Vynuluj citac odometrie`	407	`set_timer1(0); // Vynuluj citac odometrie`
-		408	`log=0; // Zacatek logu v cerne skrince`
-		409	`last_log_odo=0; // Posledni zaznam odometrie do logu`
-		410
-		411	`if(BUMPER) // Kdyz nekdo na zacatku drzi naraznik, vymaz log`
-		412	`{`
-		413	`bb_l[0]=0; // Zapis na pozici 0 vzdalenost 0`
-		414	`bb_h[0]=0;`
-		415	`SaveLog(0); // Zapis do EEPROM pocet zaznamu 0, tedy jeden zaznam`
-		416	`disp(0x55); // Blikni pro potvrzeni`
-		417	`delay_ms(200);`
-		418	`disp(0xAA);`
-		419	`delay_ms(200);`
-		420	`}`
-		421
-		422	`last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano?`
-		423	`bb=MAKE16(read_eeprom(1) & 0x7F,read_eeprom(0)); // Precti prvni zaznam`
369		424
370	`// ........................... Hlavni smycka ................................`	425	`// ........................... Hlavni smycka ................................`
371	`while(true)`	426	`while(true)`
372	`{`	427	`{`
373	`int8 pom;`	428	`int8 pom;`
374	`int8 n;`	429	`int8 n;`
375		430
376	`next_snap:`	431	`next_snap:`
377		432
378	`pom=0;`	433	`pom=0;`
379	`disable_interrupts(GLOBAL); //----------------------- Critical`	434	`disable_interrupts(GLOBAL); //----------------------- Critical`
380	`while(input(HREF)); // Preskoc 1. radku`	435	`while(input(HREF)); // Preskoc 1. radku`
381	`while(!input(HREF)); // Cekej nez se zacnou posilat pixely z 2. radky`	436	`while(!input(HREF)); // Cekej nez se zacnou posilat pixely z 2. radky`
382	`set_timer0(0); // Vynuluj pocitadlo casu`	437	`set_timer0(0); // Vynuluj pocitadlo casu`
383	`while(input(HREF)) // Po dobu vysilani radky cekej na hranu W/B`	438	`while(input(HREF)) // Po dobu vysilani radky cekej na hranu W/B`
384	`{`	439	`{`
385	`// !!!!Dodelat rozpoznani cerne cary napric pro zastaveni ?`	440	`// !!!!Dodelat rozpoznani cerne cary napric pro zastaveni ?`
386	`if(!input(PIX)) // Pokud se X-krat za sebou precetla CERNA`	441	`if(!input(PIX)) // Pokud se X-krat za sebou precetla CERNA`
387	`if(!input(PIX))`	442	`if(!input(PIX))`
388	`// if(!input(PIX))`	443	`// if(!input(PIX))`
389	`{`	444	`{`
390	`pom=get_timer0(); // Precti cas z citace casu hrany`	445	`pom=get_timer0(); // Precti cas z citace casu hrany`
391	`break;`	446	`break;`
392	`};`	447	`};`
393	`};`	448	`};`
394	`while(input(HREF)); // Pockej na shozeni signalu HREF`	449	`while(input(HREF)); // Pockej na shozeni signalu HREF`
395		450
396	`if((pom<CASMAX) && (pom>CASMIN)) cas=pom; // Orizni konce radku`	451	`if((pom<CASMAX) && (pom>CASMIN)) cas=pom; // Orizni konce radku`
397	`// Na konci obrazovaho radku to blbne. Jednak chyba od apertury`	452	`// Na konci obrazovaho radku to blbne. Jednak chyba od apertury`
398	`// a vubec to nejak na kraji nefunguje.`	453	`// a vubec to nejak na kraji nefunguje.`
399		454
400	`output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo`	455	`output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo`
401	`delay_us(1000);`	456	`delay_us(1000);`
402	`delay_us(stred);`	457	`delay_us(stred);`
403	`delay_us(stred);`	458	`delay_us(stred);`
404	`delay_us(stred);`	459	`delay_us(stred);`
405	`delay_us(cas);`	460	`delay_us(cas);`
406	`delay_us(cas);`	461	`delay_us(cas);`
407	`output_low(SERVO);`	462	`output_low(SERVO);`
408		463
-		464	`if(pom==0) // Kamera nevidi caru, poznamenej to do logu`
-		465	`{`
-		466	`if(last_log_odo>get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu`
-		467	`{`
-		468	`timer_pom=get_timer1(); // Timer se musi poznamenat atomicky`
-		469	`bb_l[log]=make8(timer_pom,0); // Zaznam`
-		470	`bb_h[log]=make8(timer_pom,1);`
-		471	`last_log_odo=timer_pom+8; // Dalsi mereni nejdrive po ujeti 24 cm`
-		472	`if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam`
-		473	`}`
-		474	`}`
-		475
409	`// Elektronicky diferencial`	476	`// Elektronicky diferencial`
410	`if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace`	477	`if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace`
411	`if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;}; // pro rizeni rychlosti motoru`	478	`if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;}; // pro rizeni rychlosti motoru`
412	`if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92`	479	`if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92`
413		480
414	`enable_interrupts(GLOBAL); //----------------------- Critical`	481	`enable_interrupts(GLOBAL); //----------------------- Critical`
415		482
416	`if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN); // Neco jako nasobeni`	483	`if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN); // Neco jako nasobeni`
417	`if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);`	484	`if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);`
418		485
419	`// r1<<=1; // Rychlost je dvojnasobna`	486	`// r1<<=1; // Rychlost je dvojnasobna`
420	`// r2<<=1; // Rozsah 2 az 184`	487	`// r2<<=1; // Rozsah 2 az 184`
421		488
422	`/* Nerozumna rychlost po cihle`	489	`/* Nerozumna rychlost po cihle`
423	`if ((stav==cihla)&&(get_timer1()>(odocounter+5))) // Snizime rychlost po ujeti`	490	`if ((stav==cihla)&&(get_timer1()>(odocounter+5))) // Snizime rychlost po ujeti`
424	`{`	491	`{`
425	`rr=rrold;`	492	`rr=rrold;`
426	`stav=pocihle;`	493	`stav=pocihle;`
427	`};`	494	`};`
428	`*/`	495	`*/`
429	`if ((stav==jizda)\|\|(stav==cihla)\|\|(stav==rozjezd)) //\|\|(stav==pocihle)) // Jizda`	496	`if ((stav==jizda)\|\|(stav==cihla)\|\|(stav==rozjezd)) //\|\|(stav==pocihle)) // Jizda`
430	`{`	497	`{`
431	`set_pwm1_duty(r1);`	498	`set_pwm1_duty(r1);`
432	`set_pwm2_duty(r2);`	499	`set_pwm2_duty(r2);`
433	`}`	500	`}`

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/camerus.c @ 249 – Rev 234 → 236