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


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

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/camerus.c – Rev 238 → 239