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

 #include ".\camerus.h"
-#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cteni z kamery
+#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
 // A/D vstupy
-#define  DALKOMER 4  // AN4/RA5 - dalkomer na cihlu
+#define  DALKOMER 4  // AN4/RA5 - proximity sensor na ruku startera
 #define  CERNA    3  // AN3/RA3
 #define  CERVENA  2  // AN2/RA2 - cervene kroutitko
 #define  ZELENA   1  // AN1/RA0 - zelene kroutitko
 #define  MODRA    0  // AN0/RA1 - modre kroutitko
 #define MOT_L  PIN_B5      // Smer otaceni leveho motoru; druhy pol je RC2
 #define MOT_R  PIN_B6      // Smer otaceni praveho motoru; druhy pol je RC1
 #define DATA   PIN_B2      // K modulu LEDbar data
 #define CP     PIN_B1      // K modulu LEDbar hodiny
 #define ODO    PIN_A4      // Ze snimace z odometrie z praveho kola
-//#define CIHLA  PIN_B0      // Vstup INT, generuje preruseni pri prekazce
+#define IRRX   PIN_B0      // Vstup INT, generuje preruseni pri prekazce
-#define IRLED  PIN_C0      // Modulovani vysilaci IR LED na detekci prekazky
+#define IRTX   PIN_C0      // Modulovani vysilaci IR LED na detekci prekazky
 #define CASMIN 6           // Rozsah radku snimace
 #define CASMAX 192
 #define CASAVR ((CASMAX+CASMIN) / 2)
+}
 #int_EXT
 EXT_isr()   // Preruseni od prekazky
+{
+   // Ujistime se, ze prijaty signal je z naseho IR vysilace
+   output_high(IRTX);   // Vypni LED na detekci prekazky
+   set_pwm1_duty(0);    // Vypni motory
+   set_pwm2_duty(0);
+   output_low(MOT_L);
+   output_low(MOT_R);
+   delay_ms(2);
+   if (!input(IRRX)) return;
+   output_low(IRTX);    // Zapni LED na detekci prekazky
+   delay_ms(10);
+   if (input(IRRX)) return;
    set_pwm1_duty(0);    // reverz (zabrzdi)
    set_pwm2_duty(0);
    output_high(MOT_L);
    output_high(MOT_R);
    delay_ms(400);
    output_low(MOT_R);
    if (stav==cihla) while(true); // Zastav na furt
    if(stav==jizda)
+   {
       int n;
+/*
       SetServo(CASAVR-CASMIN);
       set_pwm1_duty(40); // pomalu couvej
       set_pwm2_duty(40);
       output_high(MOT_L);
       output_high(MOT_R);
       delay_ms(100);
       set_pwm1_duty(0);  // Zastav
       set_pwm2_duty(0);
       output_low(MOT_L);
       output_low(MOT_R);
+*/
       delay_ms(1000);
       SetServo((CASAVR-CASMIN)-20);   // doleva
       set_pwm1_duty(150);  // vpred
       set_pwm2_duty(200);
       while(true)
+      {
          while(input(ODO));
          while(!input(ODO));
          n++;
-         if(n==10) break;
+         if(n==12) break;
+      }
       set_pwm1_duty(0);  // Zastav
       set_pwm2_duty(0);
       output_low(MOT_L);
       output_low(MOT_R);
    set_pwm1_duty(0);   // Zastav motory
    set_pwm2_duty(0);
    output_low(MOT_L);
    output_low(MOT_R);
-   output_low(IRLED);   // Zapni LED na detekci prekazky
+   output_low(IRTX);   // Zapni LED na detekci prekazky
    NightRider(1);    // Aby se poznalo, ze byl RESET
                      // Musi se pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
    //... Nastaveni kamery ...
    i2c_start();      // Soft RESET kamery
    i2c_write(0x28);
    i2c_write(0b01000001);
    i2c_stop();
 /*
-   i2c_start();      // Contrast
+   i2c_start();      // Contrast (nema podstatny vliv na obraz)
    i2c_write(0xC0);
    i2c_write(0x05);
    i2c_write(0xA0);  // 48h
    i2c_stop();
-   i2c_start();      // Brightness
-   i2c_write(0xC0);
-   i2c_write(0x06);
-   i2c_write(0x0);  // 80h //9Ah
-   i2c_stop();
-   i2c_start();      // Band Filter
+   i2c_start();      // Band Filter (pokud by byl problem se zarivkama 50Hz)
    i2c_write(0xC0);
    i2c_write(0x2D);
    i2c_write(0x04 | 0x03);
    i2c_stop();
 */
    i2c_write(118);
    i2c_stop();
    NightRider(1);    // Musi se dat cas kamere na AGC a AEC
-{ // Kalibrace kamery
+   { // Mereni expozice
-   int8 t1,t2;
+      int8 t1,t2;
-   for(offset=0;offset<=255;offset+=0x04) // Cita porad dokola
-   {
-      i2c_start();      // Brightness
+      i2c_start();      // Brightness, zacni od uplne tmy
       i2c_write(0xC0);
       i2c_write(0x06);
-      i2c_write(offset);  // 80h default
+      i2c_write(0);  // 80h default
       i2c_stop();
-      disp(offset);
       delay_ms(50);
+      for(offset=0x60;offset<=254;offset+=0x04)   // Zacni od jasu 60h
+      {
+         i2c_start();      // Brightness
+         i2c_write(0xC0);
+         i2c_write(0x06);
+         i2c_write(offset);  // 80h default
+         i2c_stop();
+         disp(offset);
+         delay_ms(50);
-      t1=0;
+         t1=0;
-      t2=0;
+         t2=0;
-      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
+         while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
-      delay_ms(5);
+         delay_ms(5);
-      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
+         while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
-      set_timer0(0);          // Vynuluj pocitadlo casu
+         set_timer0(0);          // Vynuluj pocitadlo casu
-      if(!input(PIX)) continue;
+         if(!input(PIX)) continue;
-      while(input(PIX));
+         while(input(PIX));
-      t1=get_timer0();    // Precti cas z citace casu hrany
+         t1=get_timer0();    // Precti cas z citace casu hrany
-      set_timer0(0);          // Vynuluj pocitadlo casu
+         set_timer0(0);          // Vynuluj pocitadlo casu
-      while(!input(PIX));
+         while(!input(PIX));
-      t2=get_timer0();
+         t2=get_timer0();
-      if((t1>60) && (t1<140) && (t2>5) && (t2<=10)) break;
+         if((t1>60) && (t1<140) && (t2>5) && (t2<=10)) break; // Vidis, co mas?
+         delay_ms(2);   // Preskoc druhou radku z kamery
-      delay_ms(2);
+      };
+      delay_ms(1000);   // Nech chvili na displayi zmerenou hodnotu
+   }
-}
+   set_adc_channel(CERVENA);   // --- Kroutitko pro jas ---
-delay_ms(1000);
+   delay_ms(1);
+   offset=read_adc()>>1;   // 0-127
+   offset &= 0b11111100;   // Dva nejnizsi bity ignoruj
+   disp(offset);
+   i2c_start();            // Brightness
+   i2c_write(0xC0);
+   i2c_write(0x06);
+   i2c_write(offset+0x70);  // 80h default
+   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 !!!
    cas=CASAVR-CASMIN;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
    stav=start;
    trasa=0;
       pom=0;
       while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
       set_timer0(0);          // Vynuluj pocitadlo casu
       while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
+      {
-// !!!!Dodelat rozpoznani cerne cary napric pro zastaveni
+// !!!!Dodelat rozpoznani cerne cary napric pro zastaveni ?
-         if(!input(PIX))   // Pokud se Xx za sebou precetla CERNA
+         if(!input(PIX))   // Pokud se X-krat za sebou precetla CERNA
          if(!input(PIX))
 //         if(!input(PIX))
+         {
             pom=get_timer0();    // Precti cas z citace casu hrany
             break;
       delay_us(offset);
       delay_us(cas);
       delay_us(cas);
       output_low(SERVO);
-      set_adc_channel(ZELENA);   // Kroutitko pro vykon motoru
-      Delay_ms(1);
-      rr=read_adc()>>2; //!!! // 0-31
       // 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
          INT0IF=0;                     // Zruseni predesle udalosti od startera
          enable_interrupts(INT_EXT);
          enable_interrupts(GLOBAL);
       };
-      if(stav==start)
+      if(stav==start)         // Snimkuje, toci servem a ceka na start
       if(read_adc()<128)
+      {
-         disp(0x55);
+         disp(0x80);
          while(read_adc()<128); // Cekej, dokud starter neda ruku pryc
-         set_pwm1_duty(255);
+         set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
          set_pwm2_duty(255);
-         disp(0xAA);
+         disp(0x1);
          delay_ms(200);
          stav=jizda;
+      }
-      pom=0x80;
+      pom=0x80;    // Zobrazeni pozice cary na displayi
-      for(n=CASMAX/8; n<cas; n+=CASMAX/8)
+      for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
-      {
-         pom>>=1;
-      }
       disp(pom);
    };
+}

Subversion Repositories svnkaklik

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