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


//******** Robot Camerus pro IstRobot 2007 ************
//"$Id: camerus.c 191 2007-03-16 21:34:52Z 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  DALKOMER 4  // AN4/RA5 - proximity sensor na ruku startera
#define  CERNA    3  // AN3/RA3
#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_A4      // Ze snimace z odometrie z praveho kola
#define IRRX   PIN_B0      // Vstup INT, generuje preruseni pri prekazce
#define IRTX   PIN_C0      // Modulovani vysilaci IR LED na detekci prekazky
 
#define CASMIN 6           // Rozsah radku snimace
#define CASMAX 192
#define CASAVR ((CASMAX+CASMIN) / 2)
 
#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,cil};
stavy stav;    // Kde jsme na trati
int8 cas;      // Cas hrany bila/cerna v radce
 
 
inline 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<255;n++)
   {
      output_low(MOT_L); 
      output_low(MOT_R);   
      output_high(MOT_1); 
      output_high(MOT_2);
      delay_us(800); 
      output_high(MOT_L); 
      output_high(MOT_R);   
      output_low(MOT_1); 
      output_low(MOT_2);   
      delay_us(800); 
   }
   output_low(MOT_L); // smer vpred
   output_low(MOT_R);   
   setup_ccp1(CCP_PWM); // RC1               // PWM pro motory
   setup_ccp2(CCP_PWM); // RC2
}
 
void SetServo(int8 angle)
{
   int8 n, offset;
 
   for(n=0; n<14; n++)
   {
      set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
      Delay_ms(1);
      offset=read_adc();
 
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
      delay_us(1000);
      delay_us(offset);
      delay_us(offset);
      delay_us(offset);
      delay_us(angle);
      delay_us(angle);
      output_low(SERVO);
      delay_ms(18);
   }
}
 
#int_EXT
EXT_isr()   // Preruseni od prekazky
{
   set_pwm1_duty(0);    // reverz (zabrzdi)
   set_pwm2_duty(0);
   output_high(MOT_L);  // vzad
   output_high(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;
   };
 
   SetServo((CASAVR-CASMIN));   // rovne
   delay_ms(100);    // Doba reverzace
 
   brzda();
 
   if (stav==cihla) while(true); // Zastav na furt, konec drahy
 
   if(stav==jizda)   // Objed cihlu
   {
      int n;
 
      SetServo(CASMIN);   // prudce doleva
      delay_ms(200);
 
      set_pwm1_duty(150);  // vpred
      set_pwm2_duty(200);
      output_low(MOT_L);
      output_low(MOT_R);
      n=0;
      while(true)
      {
         while(input(ODO));
         while(!input(ODO));
         n++;
         if(n==5) break;
      }
      set_pwm1_duty(0);    // reverz (zabrzdi)
      set_pwm2_duty(0);
      output_high(MOT_L);
      output_high(MOT_R);
 
      SetServo((CASAVR-CASMIN));   // rovne
      set_pwm1_duty(140);  // vpred
      set_pwm2_duty(140);
      output_low(MOT_L);
      output_low(MOT_R);
      n=0;
      while(true)
      {
         while(input(ODO));
         while(!input(ODO));
         n++;
         if(n==6) break;
      }
 
      brzda();    
 
      SetServo((CASAVR-CASMIN)+70);   // doprava
      set_pwm1_duty(140);  // vpred
      set_pwm2_duty(140);
      output_low(MOT_L);
      output_low(MOT_R);
      n=0;
      while(true)
      {
         while(input(ODO));
         while(!input(ODO));
         n++;
         if(n==6) break;
      }
 
      set_pwm1_duty(0);    // reverz (zabrzdi)
      set_pwm2_duty(0);
      output_high(MOT_L);
      output_high(MOT_R);
      delay_ms(100);
 
      brzda();    
 
      SetServo(CASMIN);   // max. doleva
      set_pwm1_duty(0);   // vzad
      set_pwm2_duty(20);
      output_low(MOT_L);
      output_high(MOT_R);
      n=0;
      while(true)
      {
         while(input(ODO));
         while(!input(ODO));
         n++;
         if(n==5) break;
      }
      set_pwm1_duty(0);    // reverz (zabrzdi)
      set_pwm2_duty(255);
      output_low(MOT_L);
      output_low(MOT_R);
      delay_ms(100);
 
      brzda();
 
      SetServo((CASAVR-CASMIN)+20);   // mirne doprava
      set_pwm1_duty(190);  // vpred
      set_pwm2_duty(190);
      output_low(MOT_L);
      output_low(MOT_R);
      n=0;
      while(true)
      {
         while(input(ODO));
         while(!input(ODO));
         n++;
         if(n==14) break;
      }
/*
      set_pwm1_duty(0);    // reverz (zabrzdi)
      set_pwm2_duty(0);
      output_high(MOT_L);
      output_high(MOT_R);
      delay_ms(100);
*/
      brzda();
      
      cas=CASMIN;  // Cara je vlevo
 
      stav=cihla;
   }
}
 
// Zobrazeni jednoho byte na modulu LEDbar
void disp(int8 x)
{
   int n;
 
   x=~x;
   for(n=0;n<=7;n++)
   {
      if (x & 1 == 1) output_high(DATA); else output_low(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);
}
 
 
void main()
{
   int8 offset;   // Promena pro ulozeni ovsetu
   int8 rr;       // Promenna na ulozeni Rozumne rychlost
   int8 r1;       // Rychlost motoru 1
   int8 r2;       // Rychlost motoru 2
   int16 trasa;   // Pocitadlo ujete vzdalenosti
 
   setup_adc_ports(ALL_ANALOG);              // Zapnuti A/D prevodniku pro cteni kroutitek
   setup_adc(ADC_CLOCK_INTERNAL);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro mereni casu hrany W/B v radce
   setup_timer_1(T1_DISABLED);
   setup_timer_2(T2_DIV_BY_16,255,1);        // Casovac PWM motoru
   setup_ccp1(CCP_PWM); // RC1               // PWM pro motory
   setup_ccp2(CCP_PWM); // RC2
   setup_comparator(NC_NC_NC_NC);
   setup_vref(FALSE);
 
   set_tris_c(0b11111000);     // Nastaveni vstup/vystup pro branu C, protoze se nedela automaticky
 
   set_pwm1_duty(0);   // Zastav motory
   set_pwm2_duty(0);
   output_low(MOT_L);
   output_low(MOT_R);
 
   output_low(IRTX);   // Zapni LED na detekci prekazky
 
   NightRider(1);    // Aby se poznalo, ze byl RESET
                     // Musi se pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
 
   //... Nastaveni kamery ...
   i2c_start();      // Soft RESET kamery
   i2c_write(0xC0);        // Pro single slave musi mit vsechny zapisy adresu C0h
   i2c_write(0x12);        // Adresa registru COMH
   i2c_write(0x80 | 0x24); // Zapis ridiciho slova
   i2c_stop();
 
   i2c_start();      // BW
   i2c_write(0xC0);
   i2c_write(0x28);
   i2c_write(0b01000001);
   i2c_stop();
 
/*
   i2c_start();      // Contrast (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=0x60;offset<(255-0x04);offset+=0x04)   // Zacni od jasu 60h
      {
         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()>>1;   // 0-127
   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 !!!
 
   cas=CASAVR-CASMIN;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
   stav=start;
   trasa=0;
 
   // ... Hlavni smycka ...
   while(true)
   {
      int8 pom;
      int8 n;
 
      pom=0;
      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
      set_timer0(0);          // Vynuluj pocitadlo casu
      while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
      {
// !!!!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.
 
      set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
      Delay_ms(1);
      offset=read_adc();
 
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
      delay_us(1000);
      delay_us(offset);
      delay_us(offset);
      delay_us(offset);
      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
 
      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
 
      if ((stav==jizda)||(stav==cihla))      // Jizda
      {
         set_pwm1_duty(r1);
         set_pwm2_duty(r2);
         trasa++;
      }
      else
      {
         set_pwm1_duty(0);       // Zastaveni
         set_pwm2_duty(0);
      }
 
      set_adc_channel(DALKOMER);   // Prepni A/D prevodnik na detektor cihly
      Delay_ms(1);
 
      if((stav==jizda)&&(trasa>50)) // musi to alespon 1s jet
      {
         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);
      };
 
      if(stav==start)         // Snimkuje, toci servem a ceka na start
      if(read_adc()<128)
      {
         disp(0x80);
         while(read_adc()<128); // Cekej, dokud starter neda ruku pryc
         set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
         set_pwm2_duty(255);
         disp(0x1);
         delay_ms(400);
         stav=jizda;
      }
 
      pom=0x80;    // Zobrazeni pozice cary na displayi
      for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
      disp(pom);
   };
}

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

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/camerus.c – Rev 190 → 191