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

 //********* Robot Camerus pro IstRobot 2007 ************
-//"$Id: camerus.c 238 2007-04-17 21:44:53Z kakl $"
+//"$Id: camerus.c 239 2007-04-18 21:51:25Z 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     24       // Rozumna rychlost pro objizdeni cihly
+#define RR_CIHLA     30       // Rozumna rychlost pro objizdeni cihly
-#define RR_PRERUSENI 18       // Rozumna rychlost pro priblizeni se k preruseni
+#define RR_PRERUSENI 20       // Rozumna rychlost pro priblizeni se k preruseni
-#define BRZDNA_DRAHA 8        // Jak daleko pred problemem se zacne brzdit
+#define BRZDNA_DRAHA 0x20        // Jak daleko pred problemem se zacne brzdit
+#define ODODO1 0x37
+#define ODODO2 0x5E
 // Adresy IIC periferii
 #define CAMERA_ADR   0xC0
 #define SONAR_ADR    0xE0
 #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
+#define MAXLOG 0x10        // Maximalni pocet zaznamu v logu
-#if  MAXLOG>(EEMAX/2-1)
+#if  MAXLOG>(EEMAX/3)
    #error  Prekrocena velikost EEPROM
 #endif
 #define OFFSETO   0x9F //0x9F         // Vystredeni serva pro objeti prekazky
 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 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
 // Zobrazeni jednoho byte na modulu LEDbar
 inline void disp(int8 x)
+{
    int n;
+}
 // Zaznam LOGu do EEPROM
 void SaveLog(int8 log)
+{
-   int8 n;
+   int8 n,i;
+   i=0;
-   for(n=0;n<=(log*2);n+=2)   // Ulozeni Black Boxu do EEPROM
+   for(n=0;n<=(log*3);n+=3)   // Ulozeni Black Boxu do EEPROM
+   {
-      write_eeprom(n,bb_h[n/2]);
+      write_eeprom(n,bb_f[i]);
+      write_eeprom(n+1,bb_h[i]);
-      write_eeprom(n+1,bb_l[n/2]);
+      write_eeprom(n+2,bb_l[i]);
+      i++;
    };
    write_eeprom(EEMAX,log);  // Zapis poctu zaznamu na konec EEPROM
+}
 // Brzdeni motorama stridou 1:1
 void SetServo(int8 angle)
+{
    int8 n;
-   for(n=0; n<14; n++)
+   for(n=0; n<10; n++)
+   {
       output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
       delay_us(1000);
       delay_us(stred);
       delay_us(stred);
    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)|0x80;  // Cihla ma nastaven nejvyssi bit
+      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
    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);
+      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)
+      {
    i2c_stop();
    delay_ms(1000);   // Nech hodnotu chvili na displayi
    set_adc_channel(ZELENA);   // --- Kroutitko pro vykon motoru ---
    delay_ms(1);
-   rr=read_adc()>>2; // 0-31  // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni !!!
+   rr=read_adc()>>2; // 0-63  // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!
    rrold=rr;
    cas=CASAVR-CASMIN;   // Inicializace promenych, aby neslo servo za roh
                         // a aby se to rozjelo jeste dneska
    stav=start;          // Jsme na startu
    last_log_odo=0;      // Posledni zaznam odometrie do logu
    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;
       int8 gap;
       int16 ododo;
       gap=0;      // Vynuluj pocitadlo preruseni
 next_snap:
       pom=0;
-      disable_interrupts(GLOBAL);  //----------------------- Critical
+      disable_interrupts(GLOBAL);  //----------------------- Critical Section
       while(input(HREF));     // Preskoc 1. radku
       while(!input(HREF));    // Cekej nez se zacnou posilat pixely z 2. radky
       set_timer0(0);          // Vynuluj pocitadlo casu
       while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
+      {
       delay_us(stred);
       delay_us(cas);
       delay_us(cas);
       output_low(SERVO);
+      // Elektronicky diferencial 1. cast
+      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
+      enable_interrupts(GLOBAL);    //----------------------- End Critical Section
       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
+         {
             gap++;
-            if(gap>=3) // Trva preruseni alespon 3 snimky?
-            {
-               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
-               rr=rrold;      // Preruseni skoncilo, vrat Rozumnou Rychlost
-            }
+         }
+      }
+      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
+            if(log<MAXLOG) log++;         // Ukazatel na dalsi zaznam
+            rr=rrold;      // Preruseni cary skoncilo, znovu jed Rozumnou Rychlosti
+         }
-      else gap=0;
+         gap=0;
+      }
 //ODODO
       ododo=get_timer1();
-      if((ododo>0x1C)&&(ododo<0x38)) rr=RR_PRERUSENI;
+      if((ododo>odo1)&&(ododo<(odo1+8))) rr=RR_PRERUSENI;
-      if((ododo>0x4F)&&(ododo<0x68)) rr=RR_PRERUSENI;
+      if((ododo>odo2)&&(ododo<(odo2+8))) 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
-      enable_interrupts(GLOBAL);    //----------------------- Critical
+      // 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);
+      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
+      {
          if(BUMPER) // Sakra, do neceho jsme narazili a nevideli jsme to!
+         {
             bum();
             SaveLog(log-1);      // Zapis Black Boxu do EEPROM
+            set_pwm1_duty(140);   // pomalu vpred
+            set_pwm2_duty(140);
-            output_low(MOT_L);   // Vypni motory
+            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 @ 249 – Rev 238 → 239