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


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

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/camerus.c @ 360 – Rev 228 → 229