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

 //********* Robot Camerus pro IstRobot 2007 ************
-//"$Id: camerus.c 234 2007-04-16 21:19:54Z kaklik $"
+//"$Id: camerus.c 236 2007-04-17 14:40:34Z 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_PRERUSENI 20       // Rozumna rychlost pro priblizeni se k preruseni
+#define BRZDNA_DRAHA 8        // Jak daleko pred problemem se zacne brzdit
-#define CAMERA_adr   0xC0
+#define CAMERA_ADR   0xC0
-#define SONAR_adr    0xE0
+#define SONAR_ADR    0xE0
 // A/D vstupy
 #define  RMAX     4  // AN4/RA5 - leve cidlo na vyjeti z cary
 #define  LMAX     3  // AN3/RA3 - prave cidlo na vyjeti z cary
 #define  CERVENA  2  // AN2/RA2 - cervene kroutitko
 #define MOT_2     PIN_C2      //
 #define DATA      PIN_B2      // K modulu LEDbar data
 #define CP        PIN_B1      // K modulu LEDbar hodiny
 //#define ODO       PIN_C0      // Ze snimace z odometrie z praveho kola na TIMER1
                                 // Jeden impuls je 31,25mm
-#define IRRX      PIN_B0      // Vstup INT, generuje preruseni pri prekazce
+#define IRRX      !input(PIN_B0)   // Vstup INT, generuje preruseni pri prekazce
 #define IRTX      PIN_B3      // Modulovani vysilaci IR LED na detekci prekazky
 #define PROXIMITY PIN_C7      // Cidlo kratkeho dosahu na cihlu
+#define BUMPER    !input(PIN_A4)  // Naraznik
 #define CASMIN 6           // Rozsah radku snimace
 #define CASMAX 192
 #define CASAVR ((CASMAX+CASMIN) / 2)
+#define EEMAX  255         // Konec EEPROM
+#define MAXLOG 0x30        // Maximalni pocet zaznamu v logu
+#if  MAXLOG>(EEMAX/2-1)
+   #error  Prekrocena velikost EEPROM
+#endif
 #define OFFSETO   0x9F //0x9F         // Vystredeni serva pro objeti prekazky
 #define THR 90    // Threshold pro UV cidla na caru
 #byte INTCON      = 0x0B         // Interrupt configuration register
 enum stavy {start,rozjezd,jizda,cihla,pocihle,cil};
 stavy stav;       // Kde jsme na trati
 int8 cas;         // Cas hrany bila/cerna v radce
 int8 stred;       // Vystredeni kolecka
-int16 odocounter;  // Zaznamenani aktualniho stavu pocitadla odometrie
+int16 odocounter; // Zaznamenani aktualniho stavu pocitadla odometrie
+int16 last_log_odo; // Posledni stav odometrie poznamenany do logu
+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 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 rr;          // Promenna na ulozeni Rozumne rychlost
 int8 rrold;
 // Zobrazeni jednoho byte na modulu LEDbar
 inline void disp(int8 x)
+{
    int n;
    for(n=0;n<=7;n++)
+   {
-      if (x & 1 == 1) output_low(DATA); else output_high(DATA);
+      if (bit_test(x,0)) output_low(DATA); else output_high(DATA);
       output_high(CP);
       x>>=1;
       output_low(CP);
+   }
+}
    delay_ms(40);
    i=0;
    disp(i);
+}
+// Zaznam LOGu do EEPROM
+void SaveLog(int8 log)
+{
+   int8 n;
+   for(n=0;n<=(log*2);n+=2)   // Ulozeni Black Boxu do EEPROM
+   {
+      write_eeprom(n,bb_h[n/2]);
+      write_eeprom(n+1,bb_l[n/2]);
+   };
+   write_eeprom(EEMAX,log);  // Zapis poctu zaznamu na konec EEPROM
+}
 // Brzdeni motorama stridou 1:1
 void brzda()
+{
    int8 n,i;
    output_high(MOT_L);
    output_low(MOT_R);
    // Ujistime se, ze prijaty signal je z naseho IR vysilace
    output_high(IRTX);   // Vypni LED na detekci prekazky
    delay_ms(2);
-   if (!input(IRRX))    // stale nas signal?
+   if (IRRX)    // stale nas signal?
+   {
       output_low(MOT_L); // je odraz -> vpred
       output_low(MOT_R);
       return;
    };
    output_low(IRTX);    // Zapni LED na detekci prekazky
    delay_ms(10);
-   if (input(IRRX))     // stale nas signal?
+   if (!IRRX)     // stale nas signal?
+   {
       output_low(MOT_L); // neni odraz -> vpred
       output_low(MOT_R);
       return;
    };
+   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)|0x80;  // Cihla ma nastaven nejvyssi bit
+   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
+   {
       #include ".\objizdka_L.c"
-   }
+   };
+   last_log_odo=get_timer1()+16;  // Pul metru po cihle nezaznamenavej do LOGu
+}
 //---------------------------------- MAIN --------------------------------------
 void main()
    NightRider(1);    // Aby se poznalo, ze byl RESET
                      // taky se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
    //... Nastaveni sonaru ...
    i2c_start();
-   i2c_write(SONAR_adr);
+   i2c_write(SONAR_ADR);
    i2c_write(0x02);  // dosah
    i2c_write(0x03);  // n*43mm
    i2c_stop();
    i2c_start();
-   i2c_write(SONAR_adr);
+   i2c_write(SONAR_ADR);
    i2c_write(0x01);  // zesileni
    i2c_write(0x01);  // male, pro eliminaci echa z minuleho mereni
    i2c_stop();
 // pro ladeni sonaru
+   }
 */
    //... Nastaveni kamery ...
    i2c_start();      // Soft RESET kamery
-   i2c_write(CAMERA_adr);        // Pro single slave musi mit vsechny zapisy adresu C0h
+   i2c_write(CAMERA_ADR);        // Pro single slave musi mit vsechny zapisy adresu C0h
    i2c_write(0x12);        // Adresa registru COMH
    i2c_write(0x80 | 0x24); // Zapis ridiciho slova
    i2c_stop();
    i2c_start();      // BW
-   i2c_write(CAMERA_adr);
+   i2c_write(CAMERA_ADR);
    i2c_write(0x28);
    i2c_write(0b01000001);
    i2c_stop();
 /*
    i2c_write(0x04 | 0x03);
    i2c_stop();
 */
    i2c_start();      // Fame Rate
-   i2c_write(CAMERA_adr);
+   i2c_write(CAMERA_ADR);
    i2c_write(0x2B);
    i2c_write(0x00);  // cca 17ms (puvodni hodnota 5Eh = 20ms)
    i2c_stop();
    i2c_start();      // VSTRT
-   i2c_write(CAMERA_adr);
+   i2c_write(CAMERA_ADR);
    i2c_write(0x19);
    i2c_write(118);   // prostredni radka
    i2c_stop();
    i2c_start();      // VEND
-   i2c_write(CAMERA_adr);
+   i2c_write(CAMERA_ADR);
    i2c_write(0x1A);
    i2c_write(118);
    i2c_stop();
    NightRider(1);    // Musi se dat cas kamere na AGC a AEC
    { // Mereni expozice
       int8 t1,t2;
       i2c_start();      // Brightness, zacni od uplne tmy
-      i2c_write(CAMERA_adr);
+      i2c_write(CAMERA_ADR);
       i2c_write(0x06);
       i2c_write(0);  // 80h default
       i2c_stop();
       delay_ms(50);
       for(offset=0x04;offset<(255-0x04);offset+=0x04)   // Zacni od jasu 10h
+      {
          i2c_start();      // Brightness
-         i2c_write(CAMERA_adr);
+         i2c_write(CAMERA_ADR);
          i2c_write(0x06);
          i2c_write(offset);  // 80h default
          i2c_stop();
          disp(offset);
          delay_ms(50);
    cas=CASAVR-CASMIN;   // Inicializace promenych, aby neslo servo za roh
                         // a aby se to rozjelo jeste dneska
    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;
       delay_us(stred);
       delay_us(cas);
       delay_us(cas);
       output_low(SERVO);
+      if(pom==0) // Kamera nevidi caru, poznamenej to do logu
+      {
+         if(last_log_odo>get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu
+         {
+            timer_pom=get_timer1();          // Timer se musi poznamenat atomicky
+            bb_l[log]=make8(timer_pom,0); // Zaznam
+            bb_h[log]=make8(timer_pom,1);
+            last_log_odo=timer_pom+8;     // Dalsi mereni nejdrive po ujeti 24 cm
+            if(log<MAXLOG) log++;         // Ukazatel na dalsi zaznam
+         }
+      }
       // 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
       pom=0x80;    // Zobrazeni pozice cary na displayi
       for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
       disp(pom);
-      while(true) // Ve zbytku casu snimku cti krajni UV senzory
+      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);
+            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
          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
          for(n=0;n<20;n++) if(input(HREF)) goto next_snap;

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/camerus.c @ 258 – Rev 234 → 236