Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

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