Subversion Repositories svnkaklik

Rev

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

Rev 247 Rev 249
Line 1... Line 1...
1
//********* Robot Camerus pro IstRobot 2007 ************
1
//********* Robot Camerus pro IstRobot 2007 ************
2
//"$Id: camerus.c 247 2007-04-22 19:31:58Z kakl $"
2
//"$Id: camerus.c 249 2007-04-23 12:26:15Z kakl $"
3
//*****************************************************
3
//*****************************************************
4
 
4
 
5
#include ".\camerus.h"
5
#include ".\camerus.h"
6
 
6
 
7
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
7
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
8
 
8
 
9
// Rychlostni konstanty
9
// Rychlostni konstanty
10
#define RR_CIHLA     30       // Rozumna rychlost pro objizdeni cihly
10
#define RR_CIHLA     30       // Rozumna rychlost pro objizdeni cihly
11
#define RR_PRERUSENI 20       // Rozumna rychlost pro priblizeni se k preruseni
11
#define RR_PRERUSENI 20       // Rozumna rychlost pro priblizeni se k preruseni
12
#define BRZDNA_DRAHA 0x20        // Jak daleko pred problemem se zacne brzdit
12
#define BRZDNA_DRAHA 0x20     // Jak daleko pred problemem se zacne brzdit
13
#define ODODO1 0xFFF
13
#define ODODO_CIHLA       0xFFF
-
 
14
#define ODODO_TUNEL       0xFFF
14
#define ODODO2 0xFFF
15
#define ODODO_PRERUSENI   0xFFF
15
 
16
 
16
// Adresy IIC periferii
17
// Adresy IIC periferii
17
#define COMPAS_ADR   0xC0
18
#define COMPAS_ADR   0xC0
18
#define CAMERA_ADR   0xDC
19
#define CAMERA_ADR   0xDC
19
#define SONAR_ADR    0xE0
20
#define SONAR_ADR    0xE0
Line 75... Line 76...
75
int16 last_log;   // Cislo posledniho zaznamu v logu v EEPROM
76
int16 last_log;   // Cislo posledniho zaznamu v logu v EEPROM
76
int8 bb_h[MAXLOG];  // Cerna skrinka MSB
77
int8 bb_h[MAXLOG];  // Cerna skrinka MSB
77
int8 bb_l[MAXLOG];  // Cerna skrinka LSB
78
int8 bb_l[MAXLOG];  // Cerna skrinka LSB
78
int8 bb_f[MAXLOG];  // Cerna skrinka priznak (typ zaznamu)
79
int8 bb_f[MAXLOG];  // Cerna skrinka priznak (typ zaznamu)
79
int8 log;         // Pocitadlo pro cernou skrinku
80
int8 log;         // Pocitadlo pro cernou skrinku
80
int16 bb;         // Posledni nactena udalost z Black Boxu
-
 
81
int16 timer_pom;  // Pomocna promenna pro atomicke nacteni hodnoty timeru1
-
 
82
int8 rr;          // Promenna na ulozeni Rozumne rychlost
81
int8 rr;          // Promenna na ulozeni Rozumne rychlost
83
int8 rrold;
82
int8 rrold;
84
int16 odo1, odo2; // Problemy na trati
83
int16 odo_preruseni, odo_cihla, odo_tunel; // Problemy na trati
85
 
84
 
86
// Zobrazeni jednoho byte na modulu LEDbar
85
// Zobrazeni jednoho byte na modulu LEDbar
87
inline void disp(int8 x)
86
inline void disp(int8 x)
88
{
87
{
89
   int n;
88
   int n;
Line 138... Line 137...
138
      i++;
137
      i++;
139
   };
138
   };
140
   write_eeprom(EEMAX,log);  // Zapis poctu zaznamu na konec EEPROM
139
   write_eeprom(EEMAX,log);  // Zapis poctu zaznamu na konec EEPROM
141
}
140
}
142
 
141
 
-
 
142
// Zaznam do Logu do RAM
-
 
143
void LogLog(int8 flag, int16 gap)
-
 
144
{
-
 
145
   int16 timer_pom;
-
 
146
   
-
 
147
   timer_pom=get_timer1();          // Timer se musi vycist atomicky
-
 
148
   bb_l[log]=make8(timer_pom,0); // Zaznam
-
 
149
   bb_h[log]=make8(timer_pom,1);
-
 
150
   bb_f[log]=flag;   // Typ zaznamu
-
 
151
   if(log<MAXLOG) log++;         // Ukazatel na dalsi zaznam
-
 
152
   last_log_odo=timer_pom+gap;     // Dalsi mereni nejdrive po ujeti def. vzdalenosti
-
 
153
   rr=rrold;      // Problem skoncil, znovu jed Rozumnou Rychlosti
-
 
154
}
-
 
155
 
-
 
156
void ReadBlackBox()
-
 
157
{
-
 
158
   last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano?
-
 
159
   {
-
 
160
      int8 n,i;
-
 
161
      
-
 
162
      i=0;
-
 
163
      for(n=0;n<=last_log;n++)
-
 
164
      {
-
 
165
         if(read_eeprom(i)==0) odo_tunel=MAKE16(read_eeprom(i+2),read_eeprom(i+1));
-
 
166
         if(read_eeprom(i)==0xFF) odo_cihla=MAKE16(read_eeprom(i+2),read_eeprom(i+1));
-
 
167
         if((read_eeprom(i)>0) && (read_eeprom(i)<0xFF)) odo_preruseni=MAKE16(read_eeprom(i+2),read_eeprom(i+1));
-
 
168
      }
-
 
169
   }   
-
 
170
}
-
 
171
 
-
 
172
 
143
// Brzdeni motorama stridou 1:1
173
// Brzdeni motorama stridou 1:1
144
void brzda()
174
void brzda()
145
{
175
{
146
   int8 n,i;
176
   int8 n,i;
147
 
177
 
Line 207... Line 237...
207
   output_high(MOT_R);
237
   output_high(MOT_R);
208
   disp(0xA5);
238
   disp(0xA5);
209
   SetServo(CASAVR-CASMIN);
239
   SetServo(CASAVR-CASMIN);
210
}
240
}
211
 
241
 
-
 
242
#include ".\diag.c" 
-
 
243
 
212
//---------------------------- INT --------------------------------
244
//---------------------------- INT --------------------------------
213
#int_EXT
245
#int_EXT
214
EXT_isr()   // Preruseni od prekazky
246
EXT_isr()   // Preruseni od prekazky
215
{
247
{
216
   unsigned int8 bearing, bearing_offset, delta_bearing;
248
   unsigned int8 bearing, bearing_offset, delta_bearing;
Line 230... Line 262...
230
   };
262
   };
231
   output_low(IRTX);    // Zapni LED na detekci prekazky
263
   output_low(IRTX);    // Zapni LED na detekci prekazky
232
 
264
 
233
   i2c_start();     // Cteni kompasu
265
   i2c_start();     // Cteni kompasu
234
   i2c_write(COMPAS_ADR);
266
   i2c_write(COMPAS_ADR);
235
   i2c_write(0x1);
267
   i2c_write(0x1);   // 0-255 (odpovida 0-359)
236
   i2c_stop();
268
   i2c_stop();
237
   i2c_start();
269
   i2c_start();
238
   i2c_write(COMPAS_ADR+1);
270
   i2c_write(COMPAS_ADR+1);
239
   bearing_offset=i2c_read(0);
271
   bearing_offset=i2c_read(0); // Poznamenej hodnotu pred cihlou
240
   i2c_stop();
272
   i2c_stop();
241
 
273
 
242
   delay_ms(9);
274
   delay_ms(9);
243
   if (!IRRX)     // stale nas signal?
275
   if (!IRRX)     // stale nas signal?
244
   {
276
   {
Line 248... Line 280...
248
   };
280
   };
249
 
281
 
250
   rr=rrold;      // Po cihle se pojede opet Rozumnou Rychlosti
282
   rr=rrold;      // Po cihle se pojede opet Rozumnou Rychlosti
251
   if(stav!=cihla)
283
   if(stav!=cihla)
252
   {
284
   {
253
      timer_pom=get_timer1();       // Obsah Timeru1 se musi nacist atomicky
-
 
254
      bb_l[log]=make8(timer_pom,0); // Zaznam do logu
-
 
255
      bb_h[log]=make8(timer_pom,1);
-
 
256
      bb_f[log]=0xFF;   // Cihla
285
      LogLog(0xFF,3);   // Cihla
257
      if(log<MAXLOG) log++;        // Ukazatel na dalsi zaznam
-
 
258
   };
286
   };
259
 
287
 
260
//!!!   if(stav==cihla) while(true); // Zastav na furt, konec drahy
288
//!!!   if(stav==cihla) while(true); // Zastav na furt, konec drahy
261
//   if(stav==cihla) return; // Po druhe nic neobjizdej
289
//   if(stav==cihla) return; // Po druhe nic neobjizdej
262
// Pozor na rozjezd
290
// Pozor na rozjezd
Line 293... Line 321...
293
   output_low(MOT_L);   // Nastav smer vpred
321
   output_low(MOT_L);   // Nastav smer vpred
294
   output_low(MOT_R);
322
   output_low(MOT_R);
295
 
323
 
296
   disp(0);    // Zhasni LEDbar
324
   disp(0);    // Zhasni LEDbar
297
 
325
 
298
   if(BUMPER)  // Kdyz nekdo na zacatku drzi naraznik, vymaz log
326
   if(BUMPER)  // Kdyz nekdo na zacatku drzi naraznik, vymaz log a spust diagnostiku
299
   {
327
   {
300
      int8 n;
328
      diag();
301
 
-
 
302
      for(n=0;n<255;n++) write_eeprom(n,0);
-
 
303
      bb_l[0]=0;        // Zapis na pozici 0 vzdalenost 0
-
 
304
      bb_h[0]=0;
-
 
305
      SaveLog(0);       // Zapis do EEPROM pocet zaznamu 0, tedy jeden zaznam
-
 
306
      for(n=0;n<=4;n++)
-
 
307
      {
-
 
308
         disp(0x55);       // Blikni pro potvrzeni
-
 
309
         delay_ms(200);
-
 
310
         disp(0xAA);
-
 
311
         delay_ms(200);
-
 
312
      };
-
 
313
      while(true)
-
 
314
      {
-
 
315
         if(!IRRX)
-
 
316
         {
-
 
317
            int8 ble;
-
 
318
         
-
 
319
            i2c_start();     // Cteni kompasu
-
 
320
            i2c_write(COMPAS_ADR);
-
 
321
            i2c_write(0x1);
-
 
322
            i2c_stop();
-
 
323
            i2c_start();
-
 
324
            i2c_write(COMPAS_ADR+1);
-
 
325
            ble=i2c_read(0);
-
 
326
            i2c_stop();
-
 
327
            disp(ble);
-
 
328
            delay_ms(200);
-
 
329
         }
-
 
330
         else
-
 
331
         {
-
 
332
            disp(0x81);       // Diagnostika celniho IR senzoru na prekazku
-
 
333
            delay_ms(200);
-
 
334
         }
-
 
335
      }
-
 
336
   }
329
   }
337
 
330
 
338
   output_low(IRTX);   // Zapni LED na detekci prekazky
331
   output_low(IRTX);   // Zapni LED na detekci prekazky
339
 
332
 
340
   NightRider(1);    // Aby se poznalo, ze byl RESET
333
   NightRider(1);    // Zablikej, aby se poznalo, ze byl RESET
341
                     // taky se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
334
                     // Zaroven se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
342
 
335
 
343
   //... Nastaveni sonaru ...
336
   //... Nastaveni sonaru ...
344
   i2c_start();
337
   i2c_start();
345
   i2c_write(SONAR_ADR);
338
   i2c_write(SONAR_ADR);
346
   i2c_write(0x02);  // dosah
339
   i2c_write(0x02);  // dosah
Line 350... Line 343...
350
   i2c_write(SONAR_ADR);
343
   i2c_write(SONAR_ADR);
351
   i2c_write(0x01);  // zesileni
344
   i2c_write(0x01);  // zesileni
352
   i2c_write(0x01);  // male, pro eliminaci echa z minuleho mereni
345
   i2c_write(0x01);  // male, pro eliminaci echa z minuleho mereni
353
   i2c_stop();
346
   i2c_stop();
354
 
347
 
355
// pro ladeni sonaru
-
 
356
/*
-
 
357
   while(true)
-
 
358
   {
-
 
359
         i2c_start();     // Sonar Ping
-
 
360
      i2c_write(0xE0);
-
 
361
      i2c_write(0x0);
-
 
362
      i2c_write(0x51);  // 50 mereni v palcich, 51 mereni v cm, 52 v us
-
 
363
      i2c_stop();
-
 
364
      delay_ms(100);
-
 
365
      i2c_start();     // Odraz ze sonaru
-
 
366
      i2c_write(0xE0);
-
 
367
      i2c_write(0x3);
-
 
368
      i2c_stop();
-
 
369
      i2c_start();
-
 
370
      i2c_write(0xE1);
-
 
371
      cas=i2c_read(0);
-
 
372
      i2c_stop();
-
 
373
      disp(cas);
-
 
374
   }
-
 
375
*/
-
 
376
 
-
 
377
   //... Nastaveni kamery ...
348
   //... Nastaveni kamery ...
378
   i2c_start();      // Soft RESET kamery
349
   i2c_start();      // Soft RESET kamery
379
   i2c_write(CAMERA_ADR);        // Adresa kamery
350
   i2c_write(CAMERA_ADR);        // Adresa kamery
380
   i2c_write(0x12);        // Adresa registru COMH
351
   i2c_write(0x12);        // Adresa registru COMH
381
   i2c_write(0x80 | 0x24); // Zapis ridiciho slova
352
   i2c_write(0x80 | 0x24); // Zapis ridiciho slova
Line 484... Line 455...
484
   stav=start;          // Jsme na startu
455
   stav=start;          // Jsme na startu
485
   set_timer1(0);       // Vynuluj citac odometrie
456
   set_timer1(0);       // Vynuluj citac odometrie
486
   log=0;               // Zacatek logu v cerne skrince
457
   log=0;               // Zacatek logu v cerne skrince
487
   last_log_odo=0;      // Posledni zaznam odometrie do logu
458
   last_log_odo=0;      // Posledni zaznam odometrie do logu
488
 
459
 
489
   last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano?
460
//   ReadBlackBox();    // Vycteni zaznamu z Black Boxu
490
   bb=MAKE16(read_eeprom(1) & 0x7F,read_eeprom(0)); // Precti prvni zaznam
-
 
491
 
461
 
492
   odo1=ODODO1-BRZDNA_DRAHA;
462
   odo_cihla=ODODO_CIHLA-BRZDNA_DRAHA;
493
   odo2=ODODO2-BRZDNA_DRAHA;
463
   odo_tunel=ODODO_TUNEL-BRZDNA_DRAHA;
-
 
464
   odo_preruseni=ODODO_PRERUSENI-BRZDNA_DRAHA;
494
 
465
 
495
   // ........................... Hlavni smycka ................................
466
   // ........................... Hlavni smycka ................................
496
   while(true)
467
   while(true)
497
   {
468
   {
498
      int8 pom;
469
      int8 pom;
Line 552... Line 523...
552
      }
523
      }
553
      else
524
      else
554
      {
525
      {
555
         if(gap>=2) // Trva preruseni cary alespon 2 snimky?
526
         if(gap>=2) // Trva preruseni cary alespon 2 snimky?
556
         {
527
         {
557
            timer_pom=get_timer1();          // Timer se musi vycist atomicky
-
 
558
            bb_l[log]=make8(timer_pom,0); // Zaznam
-
 
559
            bb_h[log]=make8(timer_pom,1);
-
 
560
            bb_f[log]=gap;
-
 
561
            last_log_odo=timer_pom+8;     // Dalsi mereni nejdrive po ujeti 24 cm
528
            LogLog(gap,8); // Dalsi mereni nejdrive po ujeti 24 cm
562
            if(log<MAXLOG) log++;         // Ukazatel na dalsi zaznam
-
 
563
            rr=rrold;      // Preruseni cary skoncilo, znovu jed Rozumnou Rychlosti
529
            rr=rrold;      // Preruseni cary skoncilo, znovu jed Rozumnou Rychlosti
564
         }
530
         }
565
         gap=0;
531
         gap=0;
566
      }
532
      };
567
 
533
 
-
 
534
      if(input(PROXIMITY) && ((stav==jizda)||(stav==cihla))) // Tunel
-
 
535
      {
-
 
536
         if(last_log_odo<get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu
-
 
537
         {
-
 
538
            LogLog(0,16);   // Priznak tunelu; dalsi mereni nejdrive po ujeti 48 cm
-
 
539
            rr=rrold;      // Vjeli jsme do tunelu, znovu jed rychle      
-
 
540
         }
-
 
541
      };
-
 
542
      
568
//ODODO
543
//ODODO
569
      ododo=get_timer1();
544
      ododo=get_timer1();
-
 
545
      if((ododo>odo_preruseni)&&(ododo<(odo_preruseni+8))) rr=RR_PRERUSENI;
570
      if((ododo>odo1)&&(ododo<(odo1+8))) rr=RR_PRERUSENI;
546
      if((ododo>odo_cihla)&&(ododo<(odo_cihla+8))) rr=RR_PRERUSENI;
571
      if((ododo>odo2)&&(ododo<(odo2+8))) rr=RR_PRERUSENI;
547
      if((ododo>odo_tunel)&&(ododo<(odo_tunel+8))) rr=RR_PRERUSENI;
572
 
548
 
573
      // Elektronicky diferencial 2. cast
549
      // Elektronicky diferencial 2. cast
574
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
550
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
575
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);     // rozsah 1 az 92 pro rr=0
551
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);     // rozsah 1 az 92 pro rr=0
576
                                                                    // rozsah 1 az 154 pro rr=63
552
                                                                    // rozsah 1 az 154 pro rr=63
577
 
553
 
578
//      r1<<=1;     // Rychlost je dvojnasobna
554
//      r1<<=1;     // Rychlost je dvojnasobna
579
//      r2<<=1;     // Rozsah 2 az 184
555
//      r2<<=1;     // Rozsah 2 az 184 pro rr=0
580
 
556
 
581
      if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle))      // Jizda
557
      if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle))      // Jizda
582
      {
558
      {
583
         set_pwm1_duty(r1);
559
         set_pwm1_duty(r1);
584
         set_pwm2_duty(r2);
560
         set_pwm2_duty(r2);
Line 637... Line 613...
637
            SaveLog(log-1);      // Zapis Black Boxu do EEPROM
613
            SaveLog(log-1);      // Zapis Black Boxu do EEPROM
638
            set_pwm1_duty(160);   // pomalu vpred
614
            set_pwm1_duty(160);   // pomalu vpred
639
            set_pwm2_duty(160);
615
            set_pwm2_duty(160);
640
            output_low(MOT_L);
616
            output_low(MOT_L);
641
            output_low(MOT_R);
617
            output_low(MOT_R);
-
 
618
            cas=CASAVR-CASMIN; 
642
         };
619
         };
643
         set_adc_channel(LMAX);    // Levy UV sensor
620
         set_adc_channel(LMAX);    // Levy UV sensor
644
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
621
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
645
         if(read_adc()<THR) cas=CASMIN;
622
         if(read_adc()<THR) cas=CASMIN;
646
         set_adc_channel(RMAX);    // Pravy UV sensor
623
         set_adc_channel(RMAX);    // Pravy UV sensor