Subversion Repositories svnkaklik

Rev

Rev 147 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 147 Rev 238
Line -... Line 1...
-
 
1
//********* Robot Camerus pro IstRobot 2007 ************
-
 
2
//"$Id: camerus.c 229 2007-04-09 11:41:19Z kakl $"
-
 
3
//*****************************************************
-
 
4
 
1
#include ".\camerus.h"
5
#include ".\camerus.h"
2
 
6
 
-
 
7
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
-
 
8
 
-
 
9
// A/D vstupy
-
 
10
#define  RMAX     4  // AN4/RA5 - leve cidlo na vyjeti z cary
-
 
11
#define  LMAX     3  // AN3/RA3 - prave cidlo na vyjeti z cary
-
 
12
#define  CERVENA  2  // AN2/RA2 - cervene kroutitko
-
 
13
#define  ZELENA   1  // AN1/RA0 - zelene kroutitko
-
 
14
#define  MODRA    0  // AN0/RA1 - modre kroutitko
-
 
15
 
-
 
16
// I/O
-
 
17
#define HREF      PIN_C5      // Signal HREF z kamery (v H po celou dobu radku)
-
 
18
#define PIX       PIN_C6      // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))
-
 
19
#define SERVO     PIN_B4      // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))
-
 
20
#define MOT_L     PIN_B5      // Smer otaceni leveho motoru; druhy pol je RC2
-
 
21
#define MOT_R     PIN_B6      // Smer otaceni praveho motoru; druhy pol je RC1
-
 
22
#define MOT_1     PIN_C1      // PWM vystpy motoru
-
 
23
#define MOT_2     PIN_C2      //
-
 
24
#define DATA      PIN_B2      // K modulu LEDbar data
-
 
25
#define CP        PIN_B1      // K modulu LEDbar hodiny
-
 
26
//#define ODO       PIN_C0      // Ze snimace z odometrie z praveho kola na TIMER1
-
 
27
                                // Jeden impuls je 31,25mm
-
 
28
#define IRRX      PIN_B0      // Vstup INT, generuje preruseni pri prekazce
-
 
29
#define IRTX      PIN_B3      // Modulovani vysilaci IR LED na detekci prekazky
-
 
30
#define PROXIMITY PIN_C7      // Cidlo kratkeho dosahu na cihlu
-
 
31
 
-
 
32
#define CASMIN 6           // Rozsah radku snimace
-
 
33
#define CASMAX 192
-
 
34
#define CASAVR ((CASMAX+CASMIN) / 2)
-
 
35
 
-
 
36
#define OFFSETO   0x9F //0x9F         // Vystredeni serva pro objeti prekazky
-
 
37
 
-
 
38
#define THR 90    // Threshold pro UV cidla na caru
-
 
39
 
-
 
40
#byte INTCON      = 0x0B         // Interrupt configuration register
-
 
41
   #bit GIE       = INTCON.7
-
 
42
   #bit PEIE      = INTCON.6
-
 
43
   #bit TMR0IE    = INTCON.5
-
 
44
   #bit INT0IE    = INTCON.4
-
 
45
   #bit RBIE      = INTCON.3
-
 
46
   #bit TMR0IF    = INTCON.2
-
 
47
   #bit INT0IF    = INTCON.1
-
 
48
   #bit RBIF      = INTCON.0
-
 
49
 
-
 
50
enum stavy {start,rozjezd,jizda,cihla,pocihle,cil};
-
 
51
stavy stav;       // Kde jsme na trati
-
 
52
int8 cas;         // Cas hrany bila/cerna v radce
-
 
53
int8 stred;       // Vystredeni kolecka
-
 
54
int16 odocounter;  // Zaznamenani aktualniho stavu pocitadla odometrie
-
 
55
int8 rr;       // Promenna na ulozeni Rozumne rychlost
-
 
56
int8 rrold;
-
 
57
//int8 pole_h[0x40];
-
 
58
int8 pole_l[0x40];
-
 
59
 
-
 
60
// Zobrazeni jednoho byte na modulu LEDbar
-
 
61
inline void disp(int8 x)
-
 
62
{
-
 
63
   int n;
-
 
64
 
-
 
65
   for(n=0;n<=7;n++)
-
 
66
   {
-
 
67
      if (x & 1 == 1) output_low(DATA); else output_high(DATA);
-
 
68
      output_high(CP);
-
 
69
      x>>=1;
-
 
70
      output_low(CP);
-
 
71
   }
-
 
72
}
-
 
73
 
-
 
74
// Blikani LEDbarem ve stilu Night Rider
-
 
75
void NightRider(int8 x)
-
 
76
{
-
 
77
   int n,i,j;
-
 
78
 
-
 
79
   for(j=0;j<x;j++)
-
 
80
   {
-
 
81
      i=0x01;
-
 
82
      for(n=0;n<7;n++)
-
 
83
      {
-
 
84
         disp(i);
-
 
85
         rotate_left(&i, 1);
-
 
86
         delay_ms(40);
-
 
87
      }
-
 
88
      for(n=0;n<7;n++)
-
 
89
      {
-
 
90
         disp(i);
-
 
91
         rotate_right(&i, 1);
-
 
92
         delay_ms(40);
-
 
93
      }
-
 
94
   }
-
 
95
   disp(i);
-
 
96
   delay_ms(40);
-
 
97
   i=0;
-
 
98
   disp(i);
-
 
99
}
-
 
100
 
-
 
101
// Brzdeni motorama stridou 1:1
-
 
102
void brzda()
-
 
103
{
-
 
104
   int8 n,i;
-
 
105
 
-
 
106
   set_pwm1_duty(0);    // vypni PWM
-
 
107
   set_pwm2_duty(0);
-
 
108
   setup_ccp1(CCP_OFF);
-
 
109
   setup_ccp2(CCP_OFF);
-
 
110
   for (n=0;n<200;n++)
-
 
111
   {
-
 
112
      output_low(MOT_L);
-
 
113
      output_low(MOT_R);
-
 
114
      output_high(MOT_1);
-
 
115
      output_high(MOT_2);
-
 
116
      delay_us(200);
-
 
117
      output_high(MOT_L);
-
 
118
      output_high(MOT_R);
-
 
119
      output_low(MOT_1);
-
 
120
      output_low(MOT_2);
-
 
121
      delay_us(200);
-
 
122
   }
-
 
123
   output_low(MOT_L); // smer vpred
-
 
124
   output_low(MOT_R);
-
 
125
   setup_ccp1(CCP_PWM); // RC1               // Zapni PWM pro motory
-
 
126
   setup_ccp2(CCP_PWM); // RC2
-
 
127
}
-
 
128
 
-
 
129
void SetServo(int8 angle)
-
 
130
{
-
 
131
   int8 n;
-
 
132
 
-
 
133
   for(n=0; n<14; n++)
-
 
134
   {
-
 
135
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
-
 
136
      delay_us(1000);
-
 
137
      delay_us(stred);
-
 
138
      delay_us(stred);
-
 
139
      delay_us(stred);
-
 
140
      delay_us(angle);
-
 
141
      delay_us(angle);
-
 
142
      output_low(SERVO);
-
 
143
      delay_ms(18);
-
 
144
   }
-
 
145
}
-
 
146
 
-
 
147
inline void SetServoQ(int8 angle)
-
 
148
{
-
 
149
   output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
-
 
150
   delay_us(1000);
-
 
151
   delay_us(stred);
-
 
152
   delay_us(stred);
-
 
153
   delay_us(stred);
-
 
154
   delay_us(angle);
-
 
155
   delay_us(angle);
-
 
156
   output_low(SERVO);
-
 
157
}
3
 
158
 
-
 
159
//---------------------------- INT --------------------------------
-
 
160
#int_EXT
-
 
161
EXT_isr()   // Preruseni od prekazky
-
 
162
{
-
 
163
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vypni
-
 
164
   set_pwm2_duty(0);
-
 
165
   output_high(MOT_L);
-
 
166
   output_low(MOT_R);
-
 
167
   // Ujistime se, ze prijaty signal je z naseho IR vysilace
-
 
168
   output_high(IRTX);   // Vypni LED na detekci prekazky
-
 
169
   delay_ms(2);
-
 
170
   if (!input(IRRX))    // stale nas signal?
-
 
171
   {
-
 
172
      output_low(MOT_L); // je odraz -> vpred
-
 
173
      output_low(MOT_R);
-
 
174
      return;
-
 
175
   };
-
 
176
   output_low(IRTX);    // Zapni LED na detekci prekazky
-
 
177
   delay_ms(10);
-
 
178
   if (input(IRRX))     // stale nas signal?
-
 
179
   {
-
 
180
      output_low(MOT_L); // neni odraz -> vpred
-
 
181
      output_low(MOT_R);
-
 
182
      return;
-
 
183
   };
-
 
184
 
-
 
185
//!!!   if(stav==cihla) while(true); // Zastav na furt, konec drahy
-
 
186
//   if(stav==cihla) return; // Po druhe nic neobjizdej
-
 
187
// Pozor na rozjezd
-
 
188
 
-
 
189
   if(stav==jizda)   // Objed cihlu
-
 
190
   {
-
 
191
      #include ".\objizdka_L.c"
-
 
192
   }
-
 
193
}
-
 
194
 
-
 
195
 
-
 
196
//---------------------------------- MAIN --------------------------------------
4
void main()
197
void main()
5
{
198
{
-
 
199
   int8 offset;   // Promena pro ulozeni offsetu
-
 
200
   int8 r1;       // Rychlost motoru 1
-
 
201
   int8 r2;       // Rychlost motoru 2
-
 
202
 
-
 
203
   setup_adc_ports(ALL_ANALOG);              // Zapnuti A/D prevodniku pro cteni kroutitek
6
   setup_adc_ports(NO_ANALOGS);
204
   setup_adc(ADC_CLOCK_INTERNAL);
-
 
205
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro mereni casu hrany W/B v radce
-
 
206
   setup_timer_1(T1_EXTERNAL);               // Cita pulzy z odometrie z praveho kola
-
 
207
   setup_timer_2(T2_DIV_BY_16,255,1);        // Casovac PWM motoru
-
 
208
   setup_ccp1(CCP_PWM); // RC1               // PWM pro motory
7
   setup_adc(ADC_OFF);
209
   setup_ccp2(CCP_PWM); // RC2
8
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
210
   setup_comparator(NC_NC_NC_NC);
9
   setup_timer_1(T1_DISABLED);
211
   setup_vref(FALSE);
-
 
212
 
-
 
213
   set_tris_c(0b11111001);     // Nastaveni vstup/vystup pro branu C, protoze se nedela automaticky
-
 
214
 
-
 
215
   set_pwm1_duty(0);    // Zastav motory
10
   setup_timer_2(T2_DISABLED,0,1);
216
   set_pwm2_duty(0);
-
 
217
   output_low(MOT_L);   // Nastav smer vpred
-
 
218
   output_low(MOT_R);
11
 
219
 
12
   output_high(PIN_C0);
220
   output_low(IRTX);   // Zapni LED na detekci prekazky
13
 
221
 
-
 
222
   NightRider(1);    // Aby se poznalo, ze byl RESET
-
 
223
                     // taky se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
-
 
224
 
-
 
225
 
-
 
226
/*
-
 
227
for(cas=0;cas<=0x3F;cas++)
-
 
228
{
-
 
229
   pole_h[cas]=0x80|cas;
-
 
230
   pole_l[cas]=cas;
-
 
231
}
-
 
232
for(cas=0;cas<=0x7F;cas+=2)
-
 
233
{
-
 
234
   write_eeprom(cas,pole_h[cas/2]);
-
 
235
   write_eeprom(cas+1,pole_l[cas/2]);
-
 
236
}
-
 
237
*/
-
 
238
 
-
 
239
while(true)
-
 
240
{
-
 
241
   output_high(PIN_B0);
-
 
242
   delay_ms(200);
-
 
243
   output_low(PIN_B0);
-
 
244
   delay_ms(200);   
-
 
245
}
-
 
246
 
-
 
247
   //... Nastaveni sonaru ...
14
   i2c_start();      // Reset
248
   i2c_start();
15
   i2c_write(0xC0);
249
   i2c_write(0xE0);
-
 
250
   i2c_write(0x02);  // dosah
-
 
251
   i2c_write(0x03);  // n*43mm
-
 
252
   i2c_stop();
-
 
253
   i2c_start();
16
   i2c_write(0x12);
254
   i2c_write(0xE0);
-
 
255
   i2c_write(0x01);  // zesileni
-
 
256
   i2c_write(0x01);  // male, pro eliminaci echa z minuleho mereni
-
 
257
   i2c_stop();
-
 
258
 
-
 
259
// pro ladeni sonaru
-
 
260
/*
-
 
261
   while(true)
-
 
262
   {
-
 
263
         i2c_start();     // Sonar Ping
-
 
264
      i2c_write(0xE0);
-
 
265
      i2c_write(0x0);
-
 
266
      i2c_write(0x51);  // 50 mereni v palcich, 51 mereni v cm, 52 v us
-
 
267
      i2c_stop();
-
 
268
      delay_ms(100);
-
 
269
      i2c_start();     // Odraz ze sonaru
17
   i2c_write(0x80 | 0x24);
270
      i2c_write(0xE0);
-
 
271
      i2c_write(0x3);
-
 
272
      i2c_stop();
-
 
273
      i2c_start();
-
 
274
      i2c_write(0xE1);
-
 
275
      cas=i2c_read(0);
-
 
276
      i2c_stop();
-
 
277
      disp(cas);
-
 
278
   }
-
 
279
*/
-
 
280
 
-
 
281
   //... Nastaveni kamery ...
-
 
282
   i2c_start();      // Soft RESET kamery
-
 
283
   i2c_write(0xC0);        // Pro single slave musi mit vsechny zapisy adresu C0h
-
 
284
   i2c_write(0x12);        // Adresa registru COMH
-
 
285
   i2c_write(0x80 | 0x24); // Zapis ridiciho slova
18
   i2c_stop();
286
   i2c_stop();
19
 
287
 
20
   i2c_start();      // BW
288
   i2c_start();      // BW
21
   i2c_write(0xC0);
289
   i2c_write(0xC0);
22
   i2c_write(0x28);
290
   i2c_write(0x28);
23
   i2c_write(0b01000001);
291
   i2c_write(0b01000001);
24
   i2c_stop();
292
   i2c_stop();
25
 
293
 
-
 
294
/*
26
   i2c_start();      // Contrast
295
   i2c_start();      // Contrast (nema podstatny vliv na obraz)
27
   i2c_write(0xC0);
296
   i2c_write(0xC0);
28
   i2c_write(0x05);
297
   i2c_write(0x05);
29
   i2c_write(0xFF);
298
   i2c_write(0xA0);  // 48h
30
   i2c_stop();
299
   i2c_stop();
31
 
300
 
32
   i2c_start();      // Brightness
-
 
33
   i2c_write(0xC0);
-
 
34
   i2c_write(0x06);
-
 
35
   i2c_write(0x80);
-
 
36
   i2c_stop();
-
 
37
 
-
 
38
/*
-
 
39
   i2c_start();      // Band Filter
301
   i2c_start();      // Band Filter (pokud by byl problem se zarivkama 50Hz)
40
   i2c_write(0xC0);
302
   i2c_write(0xC0);
41
   i2c_write(0x2D);
303
   i2c_write(0x2D);
42
   i2c_write(0x04 | 0x03);
304
   i2c_write(0x04 | 0x03);
43
   i2c_stop();
305
   i2c_stop();
-
 
306
*/
44
 
307
 
45
   i2c_start();      // 4-bit, (CLK/4)
308
   i2c_start();      // Fame Rate
46
   i2c_write(0xC0);
309
   i2c_write(0xC0);
47
   i2c_write(0x3E);
310
   i2c_write(0x2B);
48
   i2c_write(0x10 | 0x80);
311
   i2c_write(0x00);  // cca 17ms (puvodni hodnota 5Eh = 20ms)
49
   i2c_stop();
312
   i2c_stop();
50
*/
313
 
51
   i2c_start();      // VSTRT
314
   i2c_start();      // VSTRT
52
   i2c_write(0xC0);
315
   i2c_write(0xC0);
53
   i2c_write(0x19);
316
   i2c_write(0x19);
54
   i2c_write(45);
317
   i2c_write(118);   // prostredni radka
55
   i2c_stop();
318
   i2c_stop();
56
 
319
 
57
   i2c_start();      // VEND
320
   i2c_start();      // VEND
58
   i2c_write(0xC0);
321
   i2c_write(0xC0);
59
   i2c_write(0x1A);
322
   i2c_write(0x1A);
60
   i2c_write(45);
323
   i2c_write(118);
61
   i2c_stop();
324
   i2c_stop();
62
 
325
 
-
 
326
   NightRider(1);    // Musi se dat cas kamere na AGC a AEC
-
 
327
 
-
 
328
   { // Mereni expozice
63
   output_low(PIN_C0);
329
      int8 t1,t2;
64
 
330
 
-
 
331
      i2c_start();      // Brightness, zacni od uplne tmy
-
 
332
      i2c_write(0xC0);
-
 
333
      i2c_write(0x06);
-
 
334
      i2c_write(0);  // 80h default
-
 
335
      i2c_stop();
-
 
336
      delay_ms(50);
-
 
337
 
-
 
338
      for(offset=0x04;offset<(255-0x04);offset+=0x04)   // Zacni od jasu 10h
-
 
339
      {
-
 
340
         i2c_start();      // Brightness
-
 
341
         i2c_write(0xC0);
-
 
342
         i2c_write(0x06);
-
 
343
         i2c_write(offset);  // 80h default
-
 
344
         i2c_stop();
-
 
345
         disp(offset);
-
 
346
         delay_ms(50);
-
 
347
 
-
 
348
         t1=0;
-
 
349
         t2=0;
-
 
350
         while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
-
 
351
         delay_ms(5);
-
 
352
         while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
-
 
353
         set_timer0(0);          // Vynuluj pocitadlo casu
-
 
354
         if(!input(PIX)) continue;
-
 
355
         while(input(PIX));
-
 
356
         t1=get_timer0();    // Precti cas z citace casu hrany
-
 
357
         set_timer0(0);          // Vynuluj pocitadlo casu
-
 
358
         while(!input(PIX));
-
 
359
         t2=get_timer0();
-
 
360
 
-
 
361
         if((t1>60) && (t1<140) && (t2>5) && (t2<=10)) break; // Vidis, co mas?
-
 
362
 
-
 
363
         delay_ms(2);   // Preskoc druhou radku z kamery
-
 
364
      };
-
 
365
      delay_ms(1000);   // Nech chvili na displayi zmerenou hodnotu
-
 
366
   }
-
 
367
 
-
 
368
   set_adc_channel(CERVENA);   // --- Kroutitko pro jas ---
-
 
369
   delay_ms(1);
-
 
370
   offset=read_adc();
-
 
371
   offset &= 0b11111100;   // Dva nejnizsi bity ignoruj
-
 
372
//   offset += 0x70;         // Jas nebude nikdy nizsi
-
 
373
   disp(offset);
-
 
374
   i2c_start();            // Brightness
-
 
375
   i2c_write(0xC0);
-
 
376
   i2c_write(0x06);
-
 
377
   i2c_write(offset);  // 80h default
-
 
378
   i2c_stop();
-
 
379
   delay_ms(1000);   // Nech hodnotu chvili na displayi
-
 
380
 
-
 
381
   set_adc_channel(ZELENA);   // --- Kroutitko pro vykon motoru ---
-
 
382
   delay_ms(1);
-
 
383
   rr=read_adc()>>2; // 0-31  // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni !!!
-
 
384
   rrold=rr;
-
 
385
 
-
 
386
   cas=CASAVR-CASMIN;   // Inicializace promenych, aby neslo servo za roh
-
 
387
                        // a aby se to rozjelo jeste dneska
-
 
388
   stav=start;          // Jsme na startu
-
 
389
   set_timer1(0);       // Vynuluj citac odometrie
-
 
390
 
-
 
391
   // ........................... Hlavni smycka ................................
65
   while(true);
392
   while(true)
-
 
393
   {
-
 
394
      int8 pom;
-
 
395
      int8 n;
-
 
396
 
-
 
397
next_snap:
-
 
398
 
-
 
399
      pom=0;
-
 
400
      disable_interrupts(GLOBAL);  //----------------------- Critical
-
 
401
      while(input(HREF));     // Preskoc 1. radku
-
 
402
      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z 2. radky
-
 
403
      set_timer0(0);          // Vynuluj pocitadlo casu
-
 
404
      while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
-
 
405
      {
-
 
406
// !!!!Dodelat rozpoznani cerne cary napric pro zastaveni ?
-
 
407
         if(!input(PIX))   // Pokud se X-krat za sebou precetla CERNA
-
 
408
         if(!input(PIX))
-
 
409
//         if(!input(PIX))
-
 
410
         {
-
 
411
            pom=get_timer0();    // Precti cas z citace casu hrany
-
 
412
            break;
-
 
413
         };
-
 
414
      };
-
 
415
      while(input(HREF));      // Pockej na shozeni signalu HREF
-
 
416
 
-
 
417
      if((pom<CASMAX) && (pom>CASMIN)) cas=pom;  // Orizni konce radku
-
 
418
      // Na konci obrazovaho radku to blbne. Jednak chyba od apertury
-
 
419
      // a vubec to nejak na kraji nefunguje.
-
 
420
 
-
 
421
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
-
 
422
      delay_us(1000);
-
 
423
      delay_us(stred);
-
 
424
      delay_us(stred);
-
 
425
      delay_us(stred);
-
 
426
      delay_us(cas);
-
 
427
      delay_us(cas);
-
 
428
      output_low(SERVO);
-
 
429
 
-
 
430
      // Elektronicky diferencial
-
 
431
      if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
-
 
432
      if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
-
 
433
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
-
 
434
 
-
 
435
      enable_interrupts(GLOBAL);    //----------------------- Critical
-
 
436
 
-
 
437
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
-
 
438
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
-
 
439
 
-
 
440
//      r1<<=1;     // Rychlost je dvojnasobna
-
 
441
//      r2<<=1;     // Rozsah 2 az 184
-
 
442
 
-
 
443
/* Nerozumna rychlost po cihle
-
 
444
      if ((stav==cihla)&&(get_timer1()>(odocounter+5))) // Snizime rychlost po ujeti
-
 
445
      {
-
 
446
         rr=rrold;
-
 
447
         stav=pocihle;
-
 
448
      };
-
 
449
*/
-
 
450
      if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle))      // Jizda
-
 
451
      {
-
 
452
         set_pwm1_duty(r1);
-
 
453
         set_pwm2_duty(r2);
-
 
454
      }
-
 
455
      else
-
 
456
      {
-
 
457
         set_pwm1_duty(0);       // Zastaveni
-
 
458
         set_pwm2_duty(0);
-
 
459
      };
-
 
460
 
-
 
461
      if((stav==rozjezd)&&(get_timer1()>10)) // musi ujet alespon 31cm
-
 
462
      {
-
 
463
         ext_int_edge(H_TO_L);         // Nastav podminky preruseni od cihly
-
 
464
         INT0IF=0;                     // Zruseni predesle udalosti od startera
-
 
465
         enable_interrupts(INT_EXT);
-
 
466
         enable_interrupts(GLOBAL);
-
 
467
         stav=jizda;
-
 
468
      };
-
 
469
 
-
 
470
      if(stav==start)         // Snimkuje, toci servem a ceka na start
-
 
471
      {
-
 
472
         set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
-
 
473
         Delay_ms(1);
-
 
474
         stred=read_adc();
-
 
475
         if(!input(PROXIMITY))
-
 
476
         {
-
 
477
            disp(0x80);
-
 
478
            while(input(PROXIMITY)); // Cekej, dokud starter neda ruku pryc
-
 
479
            set_timer1(0);         // Vynuluj citac odometrie
-
 
480
            set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
-
 
481
            set_pwm2_duty(255);
-
 
482
            disp(0x01);
-
 
483
            while(get_timer1()<=4) // Ujed alespon 12cm
-
 
484
            {
-
 
485
               set_adc_channel(LMAX);    // Levy UV sensor
-
 
486
               delay_us(40);
-
 
487
               if(read_adc()<THR) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
-
 
488
               set_adc_channel(RMAX);    // Pravy UV sensor
-
 
489
               delay_us(40);
-
 
490
               if(read_adc()<THR) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
-
 
491
               cas=CASAVR-CASMIN;    // Cara je rovne
-
 
492
            };
-
 
493
            stav=rozjezd;
-
 
494
         };
-
 
495
      }
-
 
496
 
-
 
497
      pom=0x80;    // Zobrazeni pozice cary na displayi
-
 
498
      for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
-
 
499
      disp(pom);
-
 
500
 
-
 
501
      while(true) // Ve zbytku casu snimku cti krajni UV senzory
-
 
502
      {
-
 
503
         set_adc_channel(LMAX);    // Levy UV sensor
-
 
504
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
-
 
505
         if(read_adc()<THR) cas=CASMIN;
-
 
506
         set_adc_channel(RMAX);    // Pravy UV sensor
-
 
507
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
-
 
508
         if(read_adc()<THR) cas=CASMAX;
-
 
509
      };
-
 
510
   }
66
}
511
}
-
 
512