Subversion Repositories svnkaklik

Rev

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