Subversion Repositories svnkaklik

Rev

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