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

 //********* Robot Camerus pro IstRobot 2007 ************
-//"$Id: camerus.c 239 2007-04-18 21:51:25Z kakl $"
+//"$Id: camerus.c 246 2007-04-22 15:43:09Z 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 ODODO1 0x37
 #define ODODO2 0x5E
 // Adresy IIC periferii
+#define COMPAS_ADR   0xC0
-#define CAMERA_ADR   0xC0
+#define CAMERA_ADR   0xDC
 #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
 //---------------------------- INT --------------------------------
 #int_EXT
 EXT_isr()   // Preruseni od prekazky
+{
+   int8 bearing, bearing_offset, delta_bearing;
    set_pwm1_duty(0);    // zabrzdi levym kolem, prave vypni
    set_pwm2_duty(0);
    output_high(MOT_L);
    output_low(MOT_R);
    // Ujistime se, ze prijaty signal je z naseho IR vysilace
       output_low(MOT_L); // je odraz -> vpred
       output_low(MOT_R);
       return;
    };
    output_low(IRTX);    // Zapni LED na detekci prekazky
+   i2c_start();     // Cteni kompasu
+   i2c_write(COMPAS_ADR);
+   i2c_write(0x1);
+   i2c_stop();
+   i2c_start();
+   i2c_write(COMPAS_ADR+1);
+   bearing_offset=i2c_read(0);
+   i2c_stop();
-   delay_ms(10);
+   delay_ms(9);
    if (!IRRX)     // stale nas signal?
+   {
       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
       bb_l[log]=make8(timer_pom,0); // Zaznam do logu
       bb_h[log]=make8(timer_pom,1);
       bb_f[log]=0xFF;   // Cihla
       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
    set_pwm1_duty(0);    // Zastav motory
    set_pwm2_duty(0);
    output_low(MOT_L);   // Nastav smer vpred
    output_low(MOT_R);
+   disp(0);    // Zhasni LEDbar
+/*!!!!
+while(true)
+{
+   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);
+}
+//*/
    if(BUMPER)  // Kdyz nekdo na zacatku drzi naraznik, vymaz log
+   {
       int8 n;
       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
       while(true)
+      {
+         if(!IRRX)
+         {
-         disp(0x55);       // Blikni pro potvrzeni
+            disp(0x55);       // Blikni pro potvrzeni
-         delay_ms(200);
+            delay_ms(200);
-         disp(0xAA);
+            disp(0xAA);
+            delay_ms(200);
+         }
+         else
+         {
+            disp(0x81);       // Diagnostika celniho IR senzoru na prekazku
-         delay_ms(200);
+            delay_ms(200);
+         }
+      }
+   }
    output_low(IRTX);   // Zapni LED na detekci prekazky
+   }
 */
    //... Nastaveni kamery ...
    i2c_start();      // Soft RESET kamery
-   i2c_write(CAMERA_ADR);        // Pro single slave musi mit vsechny zapisy adresu C0h
+   i2c_write(CAMERA_ADR);        // Adresa kamery
    i2c_write(0x12);        // Adresa registru COMH
    i2c_write(0x80 | 0x24); // Zapis ridiciho slova
    i2c_stop();
    i2c_start();      // BW
    i2c_write(0b01000001);
    i2c_stop();
 /*
    i2c_start();      // Contrast (nema podstatny vliv na obraz)
-   i2c_write(0xC0);
+   i2c_write(CAMERA_ADR);
    i2c_write(0x05);
    i2c_write(0xA0);  // 48h
    i2c_stop();
    i2c_start();      // Band Filter (pokud by byl problem se zarivkama 50Hz)
-   i2c_write(0xC0);
+   i2c_write(CAMERA_ADR);
    i2c_write(0x2D);
    i2c_write(0x04 | 0x03);
    i2c_stop();
 */
    last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano?
    bb=MAKE16(read_eeprom(1) & 0x7F,read_eeprom(0)); // Precti prvni zaznam
    odo1=ODODO1-BRZDNA_DRAHA;
    odo2=ODODO2-BRZDNA_DRAHA;
    // ........................... Hlavni smycka ................................
    while(true)
+   {
       int8 pom;
       int8 n;
       if((ododo>odo2)&&(ododo<(odo2+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
       if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle))      // Jizda
+         {
             bum();
             SaveLog(log-1);      // Zapis Black Boxu do EEPROM
             set_pwm1_duty(140);   // pomalu vpred
             set_pwm2_duty(140);
             output_low(MOT_L);
             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;

Subversion Repositories svnkaklik

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