WebSVN – svnkaklik – Diff – /roboti/istrobot/camerus/SW/876/camerus.c

 //********* Robot Camerus pro IstRobot 2007 ************
-//"$Id: camerus.c 237 2007-04-17 14:41:35Z kakl $"
+//"$Id: camerus.c 238 2007-04-17 21:44:53Z kakl $"
 //*****************************************************
 #include ".\camerus.h"
 #USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
 // Rychlostni konstanty
-#define RR_CIHLA     20       // Rozumna rychlost pro objizdeni cihly
+#define RR_CIHLA     24       // Rozumna rychlost pro objizdeni cihly
-#define RR_PRERUSENI 20       // Rozumna rychlost pro priblizeni se k preruseni
+#define RR_PRERUSENI 18       // Rozumna rychlost pro priblizeni se k preruseni
 #define BRZDNA_DRAHA 8        // Jak daleko pred problemem se zacne brzdit
 // Adresy IIC periferii
 #define CAMERA_ADR   0xC0
 #define SONAR_ADR    0xE0
    delay_us(angle);
    delay_us(angle);
    output_low(SERVO);
+}
+// Couvni po narazu na naraznik
+inline void bum()
+{
+   set_pwm1_duty(0);    // couvni, rovne dozadu
+   set_pwm2_duty(0);
+   output_high(MOT_L);
+   output_high(MOT_R);
+   disp(0xA5);
+   SetServo(CASAVR-CASMIN);
+}
 //---------------------------- INT --------------------------------
 #int_EXT
 EXT_isr()   // Preruseni od prekazky
+{
    set_pwm1_duty(0);    // zabrzdi levym kolem, prave vypni
       output_low(MOT_L); // neni odraz -> vpred
       output_low(MOT_R);
       return;
    };
+   rr=rrold;      // Po cihle se pojede opet Rozumnou Rychlosti
+   if(stav!=cihla)
+   {
-   timer_pom=get_timer1();       // Obsah Timeru1 se musi nacist atomicky
+      timer_pom=get_timer1();       // Obsah Timeru1 se musi nacist atomicky
-   bb_l[log]=make8(timer_pom,0); // Zaznam do logu
+      bb_l[log]=make8(timer_pom,0); // Zaznam do logu
-   bb_h[log]=make8(timer_pom,1)|0x80;  // Cihla ma nastaven nejvyssi bit
+      bb_h[log]=make8(timer_pom,1)|0x80;  // Cihla ma nastaven nejvyssi bit
-   if(log<MAXLOG) log++;        // Ukazatel na dalsi zaznam
+      if(log<MAXLOG) log++;        // Ukazatel na dalsi zaznam
+   };
 //!!! Vyzkouset, ze nepreteka LOG
 //!!!   if(stav==cihla) while(true); // Zastav na furt, konec drahy
 //   if(stav==cihla) return; // Po druhe nic neobjizdej
 // Pozor na rozjezd
-   if(stav==jizda)   // Objed cihlu
+   if((stav==jizda)||(stav==cihla))   // Objed cihlu
+   {
       #include ".\objizdka_L.c"
    };
    last_log_odo=get_timer1()+16;  // Pul metru po cihle nezaznamenavej do LOGu
+}
    set_pwm1_duty(0);    // Zastav motory
    set_pwm2_duty(0);
    output_low(MOT_L);   // Nastav smer vpred
    output_low(MOT_R);
+   if(BUMPER)  // Kdyz nekdo na zacatku drzi naraznik, vymaz log
+   {
+      int8 n;
+      for(n=0;n<255;n++) write_eeprom(n,0xFF);
+      bb_l[0]=0;        // Zapis na pozici 0 vzdalenost 0
+      bb_h[0]=0;
+      SaveLog(0);       // Zapis do EEPROM pocet zaznamu 0, tedy jeden zaznam
+      while(true)
+      {
+         disp(0x55);       // Blikni pro potvrzeni
+         delay_ms(200);
+         disp(0xAA);
+         delay_ms(200);
+      }
+   }
    output_low(IRTX);   // Zapni LED na detekci prekazky
    NightRider(1);    // Aby se poznalo, ze byl RESET
                      // taky se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
    stav=start;          // Jsme na startu
    set_timer1(0);       // Vynuluj citac odometrie
    log=0;               // Zacatek logu v cerne skrince
    last_log_odo=0;      // Posledni zaznam odometrie do logu
-   if(BUMPER)  // Kdyz nekdo na zacatku drzi naraznik, vymaz log
-   {
-      bb_l[0]=0;        // Zapis na pozici 0 vzdalenost 0
-      bb_h[0]=0;
-      SaveLog(0);       // Zapis do EEPROM pocet zaznamu 0, tedy jeden zaznam
-      disp(0x55);       // Blikni pro potvrzeni
-      delay_ms(200);
-      disp(0xAA);
-      delay_ms(200);
-   }
    last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano?
    bb=MAKE16(read_eeprom(1) & 0x7F,read_eeprom(0)); // Precti prvni zaznam
    // ........................... Hlavni smycka ................................
    while(true)
+   {
       int8 pom;
       int8 n;
+      int8 gap;
+      int16 ododo;
+      gap=0;      // Vynuluj pocitadlo preruseni
 next_snap:
       pom=0;
       disable_interrupts(GLOBAL);  //----------------------- Critical
       while(input(HREF));     // Preskoc 1. radku
       delay_us(cas);
       output_low(SERVO);
       if(pom==0) // Kamera nevidi caru, poznamenej to do logu
+      {
+         if((cas>(CASMIN+30))&&(cas<(CASMAX-30))) // Nebyla minule cara moc u kraje?
-         if(last_log_odo>get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu
+         if(last_log_odo<get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu
+         {
+            gap++;
+            if(gap>=3) // Trva preruseni alespon 3 snimky?
+            {
-            timer_pom=get_timer1();          // Timer se musi poznamenat atomicky
+               timer_pom=get_timer1();          // Timer se musi poznamenat atomicky
-            bb_l[log]=make8(timer_pom,0); // Zaznam
+               bb_l[log]=make8(timer_pom,0); // Zaznam
-            bb_h[log]=make8(timer_pom,1);
+               bb_h[log]=make8(timer_pom,1);
-            last_log_odo=timer_pom+8;     // Dalsi mereni nejdrive po ujeti 24 cm
+               last_log_odo=timer_pom+8;     // Dalsi mereni nejdrive po ujeti 24 cm
-            if(log<MAXLOG) log++;         // Ukazatel na dalsi zaznam
+               if(log<MAXLOG) log++;         // Ukazatel na dalsi zaznam
+               rr=rrold;      // Preruseni skoncilo, vrat Rozumnou Rychlost
+            }
+         }
+      }
+      else gap=0;
+//ODODO
+      ododo=get_timer1();
+      if((ododo>0x1C)&&(ododo<0x38)) rr=RR_PRERUSENI;
+      if((ododo>0x4F)&&(ododo<0x68)) rr=RR_PRERUSENI;
       // Elektronicky diferencial
       if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
       if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
       if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
       if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
 //      r1<<=1;     // Rychlost je dvojnasobna
 //      r2<<=1;     // Rozsah 2 az 184
-/* Nerozumna rychlost po cihle
-      if ((stav==cihla)&&(get_timer1()>(odocounter+5))) // Snizime rychlost po ujeti
-      {
-         rr=rrold;
-         stav=pocihle;
-      };
-*/
       if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle))      // Jizda
+      {
          set_pwm1_duty(r1);
          set_pwm2_duty(r2);
+      }
       while(true) // Ve zbytku casu snimku cti krajni UV senzory a naraznik
+      {
          if(BUMPER) // Sakra, do neceho jsme narazili a nevideli jsme to!
+         {
-            set_pwm1_duty(0);    // couvni, rovne dozadu
-            set_pwm2_duty(0);
-            output_high(MOT_L);
-            output_high(MOT_R);
-            disp(0x5A);
+            bum();
-            SetServo(CASAVR-CASMIN);
             SaveLog(log-1);      // Zapis Black Boxu do EEPROM
             output_low(MOT_L);   // Vypni motory
             output_low(MOT_R);
          };
          set_adc_channel(LMAX);    // Levy UV sensor

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/camerus.c @ 239 – Rev 237 → 238