Subversion Repositories svnkaklik

Rev

Rev 259 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 259 Rev 260
Line 1... Line 1...
1 
//********* Robot Camerus pro IstRobot 2007 ************
 1 
//********* Robot Camerus pro IstRobot 2007 ************
2 
//"$Id: camerus.c 259 2007-04-27 13:24:20Z kakl $"
 2 
//"$Id: camerus.c 260 2007-04-27 13:27:40Z 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
Line 9... Line 9...
9 
// Rychlostni konstanty
 9 
// Rychlostni konstanty
10 
#define RR_CIHLA     50       // Rozumna rychlost pro objizdeni cihly
 10 
#define RR_CIHLA     50       // Rozumna rychlost pro objizdeni cihly
11 
#define RR_PRERUSENI 50       // Rozumna rychlost pro priblizeni se k preruseni
 11 
#define RR_PRERUSENI 50       // Rozumna rychlost pro priblizeni se k preruseni
12 
#define BRZDNA_DRAHA 0x15     // Jak daleko pred problemem se zacne brzdit
 12 
#define BRZDNA_DRAHA 0x15     // Jak daleko pred problemem se zacne brzdit
13 
#define TUHOS        100      // Jak dlouho se bude couvat po narazu na naraznik
 13 
#define TUHOS        100      // Jak dlouho se bude couvat po narazu na naraznik
14 
#define ODODO_CIHLA       0xFFF
 14 
#define ODODO_CIHLA       0xD0
15 
#define ODODO_TUNEL       0xFFF
 15 
#define ODODO_TUNEL       0xFFF
16 
#define ODODO_PRERUSENI   0xFFF
 16 
#define ODODO_PRERUSENI   0xFFF//0xB4
17 
 
 17 
 
18 
// Adresy IIC periferii
 18 
// Adresy IIC periferii
19 
#define COMPAS_ADR   0xC0
 19 
#define COMPAS_ADR   0xC0
20 
#define CAMERA_ADR   0xDC
 20 
#define CAMERA_ADR   0xDC
21 
#define SONAR_ADR    0xE0
 21 
#define SONAR_ADR    0xE0
Line 303... Line 303...
303 
void main()
 303 
void main()
304 
{
 304 
{
305 
   int8 offset;   // Promena pro ulozeni offsetu
 305 
   int8 offset;   // Promena pro ulozeni offsetu
306 
   int8 r1;       // Rychlost motoru 1
 306 
   int8 r1;       // Rychlost motoru 1
307 
   int8 r2;       // Rychlost motoru 2
 307 
   int8 r2;       // Rychlost motoru 2
- 
 
 308 
   int16 ble;
308 
 
 309 
 
309 
   setup_adc_ports(ALL_ANALOG);              // Zapnuti A/D prevodniku pro cteni kroutitek
 310 
   setup_adc_ports(ALL_ANALOG);              // Zapnuti A/D prevodniku pro cteni kroutitek
310 
   setup_adc(ADC_CLOCK_INTERNAL);
 311 
   setup_adc(ADC_CLOCK_INTERNAL);
311 
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro mereni casu hrany W/B v radce
 312 
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro mereni casu hrany W/B v radce
312 
   setup_timer_1(T1_EXTERNAL);               // Cita pulzy z odometrie z praveho kola
 313 
   setup_timer_1(T1_EXTERNAL);               // Cita pulzy z odometrie z praveho kola
Line 516... Line 517...
516 
      if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
 517 
      if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
517 
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
 518 
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
518 
 
 519 
 
519 
      enable_interrupts(GLOBAL);    //----------------------- End Critical Section
 520 
      enable_interrupts(GLOBAL);    //----------------------- End Critical Section
520 
 
 521 
 
- 
 
 522 
      if(pom==0) // Kamera nevidi caru
- 
 
 523 
      {
- 
 
 524 
         if((cas>(CASMIN+15))&&(cas<(CASMAX-15))) // Nebyla minule cara moc u kraje?
- 
 
 525 
         {
- 
 
 526 
            gap++;
- 
 
 527 
            if(gap>=3) // Trva preruseni cary alespon 2 snimky?
- 
 
 528 
            {
- 
 
 529 
               cas=CASAVR-CASMIN;
- 
 
 530 
//               disp(0xAA);
- 
 
 531 
            }
- 
 
 532 
         }
- 
 
 533 
      }
- 
 
 534 
      else
- 
 
 535 
      {
- 
 
 536 
         gap=0;
- 
 
 537 
      };
- 
 
 538 
 
- 
 
 539 
 
- 
 
 540 
/*
521 
      if(pom==0) // Kamera nevidi caru, poznamenej to do logu
 541 
      if(pom==0) // Kamera nevidi caru, poznamenej to do logu
522 
      {
 542 
      {
523 
         if((cas>(CASMIN+30))&&(cas<(CASMAX-30))) // Nebyla minule cara moc u kraje?
 543 
         if((cas>(CASMIN+30))&&(cas<(CASMAX-30))) // Nebyla minule cara moc u kraje?
524 
         if(last_log_odo<get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu
 544 
         if(last_log_odo<get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu
525 
         {
 545 
         {
526 
            gap++;
 546 
            gap++;
527 
         }
 547 
         }
528 
      }
 548 
      }
529 
      else
 549 
      else
530 
      {
 550 
      {
531 
         if(gap>=2) // Trva preruseni cary alespon 2 snimky?
 551 
         if(gap>=4) // Trva preruseni cary alespon 2 snimky?
532 
         {
 552 
         {
533 
            LogLog(gap,8); // Dalsi mereni nejdrive po ujeti 24 cm
 553 
            LogLog(gap,8); // Dalsi mereni nejdrive po ujeti 24 cm
534 
            rr=rrold;      // Preruseni cary skoncilo, znovu jed Rozumnou Rychlosti
 554 
            rr=rrold;      // Preruseni cary skoncilo, znovu jed Rozumnou Rychlosti
- 
 
 555 
            cas=CASAVR-CASMIN;
- 
 
 556 
            disp(0xAA);
535 
         }
 557 
         }
536 
         gap=0;
 558 
         gap=0;
537 
      };
 559 
      };
538 
 
 560 
 
539 
      if(!input(PROXIMITY) && ((stav==jizda)||(stav==cihla))) // Tunel
 561 
      if(!input(PROXIMITY) && ((stav==jizda)||(stav==cihla))) // Tunel
Line 542... Line 564...
542 
         {
 564 
         {
543 
            LogLog(0xDD,16);   // Priznak tunelu; dalsi mereni nejdrive po ujeti 48 cm
 565 
            LogLog(0xDD,16);   // Priznak tunelu; dalsi mereni nejdrive po ujeti 48 cm
544 
            rr=rrold;      // Vjeli jsme do tunelu, znovu jed rychle
 566 
            rr=rrold;      // Vjeli jsme do tunelu, znovu jed rychle
545 
         }
 567 
         }
546 
      };
 568 
      };
- 
 
 569 
*/
547 
 
 570 
 
548 
//ODODO
 571 
//ODODO
549 
      ododo=get_timer1();
 572 
      ododo=get_timer1();
550 
      if((ododo>odo_preruseni)&&(ododo<(odo_preruseni+8))) rr=RR_PRERUSENI;
 573 
      if((ododo>odo_preruseni)&&(ododo<(odo_preruseni+8))) rr=RR_PRERUSENI;
551 
      if((ododo>odo_cihla)&&(ododo<(odo_cihla+8))) rr=RR_PRERUSENI;
 574 
      if((ododo>odo_cihla)&&(ododo<(odo_cihla+8))) rr=RR_PRERUSENI;