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