Subversion Repositories svnkaklik

Rev

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

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