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