Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

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