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


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

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/camerus.c – Rev 259 → 260