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

 //********* Robot Camerus pro IstRobot 2007 ************
-//"$Id: camerus.c 247 2007-04-22 19:31:58Z kakl $"
+//"$Id: camerus.c 249 2007-04-23 12:26:15Z 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     30       // Rozumna rychlost pro objizdeni cihly
 #define RR_PRERUSENI 20       // Rozumna rychlost pro priblizeni se k preruseni
-#define BRZDNA_DRAHA 0x20        // Jak daleko pred problemem se zacne brzdit
+#define BRZDNA_DRAHA 0x20     // Jak daleko pred problemem se zacne brzdit
-#define ODODO1 0xFFF
+#define ODODO_CIHLA       0xFFF
+#define ODODO_TUNEL       0xFFF
-#define ODODO2 0xFFF
+#define ODODO_PRERUSENI   0xFFF
 // Adresy IIC periferii
 #define COMPAS_ADR   0xC0
 #define CAMERA_ADR   0xDC
 #define SONAR_ADR    0xE0
 int16 last_log;   // Cislo posledniho zaznamu v logu v EEPROM
 int8 bb_h[MAXLOG];  // Cerna skrinka MSB
 int8 bb_l[MAXLOG];  // Cerna skrinka LSB
 int8 bb_f[MAXLOG];  // Cerna skrinka priznak (typ zaznamu)
 int8 log;         // Pocitadlo pro cernou skrinku
-int16 bb;         // Posledni nactena udalost z Black Boxu
-int16 timer_pom;  // Pomocna promenna pro atomicke nacteni hodnoty timeru1
 int8 rr;          // Promenna na ulozeni Rozumne rychlost
 int8 rrold;
-int16 odo1, odo2; // Problemy na trati
+int16 odo_preruseni, odo_cihla, odo_tunel; // Problemy na trati
 // Zobrazeni jednoho byte na modulu LEDbar
 inline void disp(int8 x)
+{
    int n;
       i++;
    };
    write_eeprom(EEMAX,log);  // Zapis poctu zaznamu na konec EEPROM
+}
+// Zaznam do Logu do RAM
+void LogLog(int8 flag, int16 gap)
+{
+   int16 timer_pom;
+   timer_pom=get_timer1();          // Timer se musi vycist atomicky
+   bb_l[log]=make8(timer_pom,0); // Zaznam
+   bb_h[log]=make8(timer_pom,1);
+   bb_f[log]=flag;   // Typ zaznamu
+   if(log<MAXLOG) log++;         // Ukazatel na dalsi zaznam
+   last_log_odo=timer_pom+gap;     // Dalsi mereni nejdrive po ujeti def. vzdalenosti
+   rr=rrold;      // Problem skoncil, znovu jed Rozumnou Rychlosti
+}
+void ReadBlackBox()
+{
+   last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano?
+   {
+      int8 n,i;
+      i=0;
+      for(n=0;n<=last_log;n++)
+      {
+         if(read_eeprom(i)==0) odo_tunel=MAKE16(read_eeprom(i+2),read_eeprom(i+1));
+         if(read_eeprom(i)==0xFF) odo_cihla=MAKE16(read_eeprom(i+2),read_eeprom(i+1));
+         if((read_eeprom(i)>0) && (read_eeprom(i)<0xFF)) odo_preruseni=MAKE16(read_eeprom(i+2),read_eeprom(i+1));
+      }
+   }
+}
 // Brzdeni motorama stridou 1:1
 void brzda()
+{
    int8 n,i;
    output_high(MOT_R);
    disp(0xA5);
    SetServo(CASAVR-CASMIN);
+}
+#include ".\diag.c"
 //---------------------------- INT --------------------------------
 #int_EXT
 EXT_isr()   // Preruseni od prekazky
+{
    unsigned int8 bearing, bearing_offset, delta_bearing;
    };
    output_low(IRTX);    // Zapni LED na detekci prekazky
    i2c_start();     // Cteni kompasu
    i2c_write(COMPAS_ADR);
-   i2c_write(0x1);
+   i2c_write(0x1);   // 0-255 (odpovida 0-359)
    i2c_stop();
    i2c_start();
    i2c_write(COMPAS_ADR+1);
-   bearing_offset=i2c_read(0);
+   bearing_offset=i2c_read(0); // Poznamenej hodnotu pred cihlou
    i2c_stop();
    delay_ms(9);
    if (!IRRX)     // stale nas signal?
+   {
    };
    rr=rrold;      // Po cihle se pojede opet Rozumnou Rychlosti
    if(stav!=cihla)
+   {
-      timer_pom=get_timer1();       // Obsah Timeru1 se musi nacist atomicky
-      bb_l[log]=make8(timer_pom,0); // Zaznam do logu
-      bb_h[log]=make8(timer_pom,1);
-      bb_f[log]=0xFF;   // Cihla
+      LogLog(0xFF,3);   // Cihla
-      if(log<MAXLOG) log++;        // Ukazatel na dalsi zaznam
    };
 //!!!   if(stav==cihla) while(true); // Zastav na furt, konec drahy
 //   if(stav==cihla) return; // Po druhe nic neobjizdej
 // Pozor na rozjezd
    output_low(MOT_L);   // Nastav smer vpred
    output_low(MOT_R);
    disp(0);    // Zhasni LEDbar
-   if(BUMPER)  // Kdyz nekdo na zacatku drzi naraznik, vymaz log
+   if(BUMPER)  // Kdyz nekdo na zacatku drzi naraznik, vymaz log a spust diagnostiku
+   {
-      int8 n;
+      diag();
-      for(n=0;n<255;n++) write_eeprom(n,0);
-      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
-      for(n=0;n<=4;n++)
-      {
-         disp(0x55);       // Blikni pro potvrzeni
-         delay_ms(200);
-         disp(0xAA);
-         delay_ms(200);
-      };
-      while(true)
-      {
-         if(!IRRX)
-         {
-            int8 ble;
-            i2c_start();     // Cteni kompasu
-            i2c_write(COMPAS_ADR);
-            i2c_write(0x1);
-            i2c_stop();
-            i2c_start();
-            i2c_write(COMPAS_ADR+1);
-            ble=i2c_read(0);
-            i2c_stop();
-            disp(ble);
-            delay_ms(200);
-         }
-         else
-         {
-            disp(0x81);       // Diagnostika celniho IR senzoru na prekazku
-            delay_ms(200);
-         }
-      }
+   }
    output_low(IRTX);   // Zapni LED na detekci prekazky
-   NightRider(1);    // Aby se poznalo, ze byl RESET
+   NightRider(1);    // Zablikej, aby se poznalo, ze byl RESET
-                     // taky se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
+                     // Zaroven se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
    //... Nastaveni sonaru ...
    i2c_start();
    i2c_write(SONAR_ADR);
    i2c_write(0x02);  // dosah
    i2c_write(SONAR_ADR);
    i2c_write(0x01);  // zesileni
    i2c_write(0x01);  // male, pro eliminaci echa z minuleho mereni
    i2c_stop();
-// pro ladeni sonaru
-/*
-   while(true)
-   {
-         i2c_start();     // Sonar Ping
-      i2c_write(0xE0);
-      i2c_write(0x0);
-      i2c_write(0x51);  // 50 mereni v palcich, 51 mereni v cm, 52 v us
-      i2c_stop();
-      delay_ms(100);
-      i2c_start();     // Odraz ze sonaru
-      i2c_write(0xE0);
-      i2c_write(0x3);
-      i2c_stop();
-      i2c_start();
-      i2c_write(0xE1);
-      cas=i2c_read(0);
-      i2c_stop();
-      disp(cas);
-   }
-*/
    //... Nastaveni kamery ...
    i2c_start();      // Soft RESET kamery
    i2c_write(CAMERA_ADR);        // Adresa kamery
    i2c_write(0x12);        // Adresa registru COMH
    i2c_write(0x80 | 0x24); // Zapis ridiciho slova
    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
-   last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano?
+//   ReadBlackBox();    // Vycteni zaznamu z Black Boxu
-   bb=MAKE16(read_eeprom(1) & 0x7F,read_eeprom(0)); // Precti prvni zaznam
-   odo1=ODODO1-BRZDNA_DRAHA;
+   odo_cihla=ODODO_CIHLA-BRZDNA_DRAHA;
-   odo2=ODODO2-BRZDNA_DRAHA;
+   odo_tunel=ODODO_TUNEL-BRZDNA_DRAHA;
+   odo_preruseni=ODODO_PRERUSENI-BRZDNA_DRAHA;
    // ........................... Hlavni smycka ................................
    while(true)
+   {
       int8 pom;
+      }
       else
+      {
          if(gap>=2) // Trva preruseni cary alespon 2 snimky?
+         {
-            timer_pom=get_timer1();          // Timer se musi vycist atomicky
-            bb_l[log]=make8(timer_pom,0); // Zaznam
-            bb_h[log]=make8(timer_pom,1);
-            bb_f[log]=gap;
-            last_log_odo=timer_pom+8;     // Dalsi mereni nejdrive po ujeti 24 cm
+            LogLog(gap,8); // Dalsi mereni nejdrive po ujeti 24 cm
-            if(log<MAXLOG) log++;         // Ukazatel na dalsi zaznam
             rr=rrold;      // Preruseni cary skoncilo, znovu jed Rozumnou Rychlosti
+         }
          gap=0;
-      }
+      };
+      if(input(PROXIMITY) && ((stav==jizda)||(stav==cihla))) // Tunel
+      {
+         if(last_log_odo<get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu
+         {
+            LogLog(0,16);   // Priznak tunelu; dalsi mereni nejdrive po ujeti 48 cm
+            rr=rrold;      // Vjeli jsme do tunelu, znovu jed rychle
+         }
+      };
 //ODODO
       ododo=get_timer1();
+      if((ododo>odo_preruseni)&&(ododo<(odo_preruseni+8))) rr=RR_PRERUSENI;
-      if((ododo>odo1)&&(ododo<(odo1+8))) rr=RR_PRERUSENI;
+      if((ododo>odo_cihla)&&(ododo<(odo_cihla+8))) rr=RR_PRERUSENI;
-      if((ododo>odo2)&&(ododo<(odo2+8))) rr=RR_PRERUSENI;
+      if((ododo>odo_tunel)&&(ododo<(odo_tunel+8))) rr=RR_PRERUSENI;
       // Elektronicky diferencial 2. cast
       if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
       if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);     // rozsah 1 az 92 pro rr=0
                                                                     // rozsah 1 az 154 pro rr=63
 //      r1<<=1;     // Rychlost je dvojnasobna
-//      r2<<=1;     // Rozsah 2 az 184
+//      r2<<=1;     // Rozsah 2 az 184 pro rr=0
       if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle))      // Jizda
+      {
          set_pwm1_duty(r1);
          set_pwm2_duty(r2);
             SaveLog(log-1);      // Zapis Black Boxu do EEPROM
             set_pwm1_duty(160);   // pomalu vpred
             set_pwm2_duty(160);
             output_low(MOT_L);
             output_low(MOT_R);
+            cas=CASAVR-CASMIN;
          };
          set_adc_channel(LMAX);    // Levy UV sensor
          for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
          if(read_adc()<THR) cas=CASMIN;
          set_adc_channel(RMAX);    // Pravy UV sensor

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/camerus.c @ 249 – Rev 247 → 249