Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

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