Subversion Repositories svnkaklik

Rev

Rev 249 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log

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