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

 //******** Robot Camerus pro IstRobot 2007 ************
-//"$Id: camerus.c 198 2007-03-18 10:13:14Z kakl $"
+//"$Id: camerus.c 200 2007-03-18 20:54:40Z kakl $"
 //*****************************************************
 #include ".\camerus.h"
 #USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
 // A/D vstupy
-#define  LMAX     4  // AN4/RA5 - leve cidlo na vyjeti z cary
+#define  RMAX     4  // AN4/RA5 - leve cidlo na vyjeti z cary
-#define  RMAX     3  // AN3/RA3 - prave cidlo na vyjeti z cary
+#define  LMAX     3  // AN3/RA3 - prave cidlo na vyjeti z cary
 #define  CERVENA  2  // AN2/RA2 - cervene kroutitko
 #define  ZELENA   1  // AN1/RA0 - zelene kroutitko
 #define  MODRA    0  // AN0/RA1 - modre kroutitko
 // I/O
 #define HREF      PIN_C5      // Signal HREF z kamery (v H po celou dobu radku)
 #define PIX       PIN_C6      // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))
 #define SERVO     PIN_B4      // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))
 #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 MOT_1     PIN_C1      // PWM vystpy motoru
 #define MOT_2     PIN_C2      //
 #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 ODO       PIN_A4      // Ze snimace z odometrie z praveho kola / nove je to na RC0 na TIMER1
 #define IRRX      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 CASMIN 6           // Rozsah radku snimace
 #define CASMAX 192
 #define CASAVR ((CASMAX+CASMIN) / 2)
 #byte INTCON      = 0x0B         // Interrupt configuration register
    #bit GIE       = INTCON.7
    #bit PEIE      = INTCON.6
    #bit TMR0IE    = INTCON.5
    #bit INT0IE    = INTCON.4
    #bit RBIE      = INTCON.3
    #bit TMR0IF    = INTCON.2
    #bit INT0IF    = INTCON.1
    #bit RBIF      = INTCON.0
 enum stavy {start,rozjezd,jizda,cihla,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
+// Zobrazeni jednoho byte na modulu LEDbar
+void disp(int8 x)
+{
+   int n;
+   x=~x;
+   for(n=0;n<=7;n++)
+   {
+      if (x & 1 == 1) output_high(DATA); else output_low(DATA);
+      output_high(CP);
+      x>>=1;
+      output_low(CP);
+   }
+}
+// Blikani LEDbarem ve stilu Night Rider
+void NightRider(int8 x)
+{
+   int n,i,j;
+   for(j=0;j<x;j++)
+   {
+      i=0x01;
+      for(n=0;n<7;n++)
+      {
+         disp(i);
+         rotate_left(&i, 1);
+         delay_ms(40);
+      }
+      for(n=0;n<7;n++)
+      {
+         disp(i);
+         rotate_right(&i, 1);
+         delay_ms(40);
+      }
+   }
+   disp(i);
+   delay_ms(40);
+   i=0;
+   disp(i);
+}
+// Brzdeni motorama stridou 1:1
-inline void brzda()
+void brzda()
+{
    int8 n,i;
    set_pwm1_duty(0);    // vypni PWM
    set_pwm2_duty(0);
    setup_ccp1(CCP_OFF);
    setup_ccp2(CCP_OFF);
    for (n=0;n<100;n++)
+   {
       output_low(MOT_L);
       output_low(MOT_R);
       output_high(MOT_1);
       output_high(MOT_2);
       delay_us(800);
       output_high(MOT_L);
       output_high(MOT_R);
       output_low(MOT_1);
       output_low(MOT_2);
       delay_us(800);
+   }
    output_low(MOT_L); // smer vpred
    output_low(MOT_R);
    setup_ccp1(CCP_PWM); // RC1               // Zapni PWM pro motory
    setup_ccp2(CCP_PWM); // RC2
+}
 void SetServo(int8 angle)
+{
    int8 n;
    for(n=0; n<14; n++)
+   {
       output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
       delay_us(1000);
       delay_us(stred);
       delay_us(stred);
       delay_us(stred);
       delay_us(angle);
       delay_us(angle);
       output_low(SERVO);
       delay_ms(18);
+   }
+}
 #int_EXT
 EXT_isr()   // Preruseni od prekazky
+{
    set_pwm1_duty(0);    // reverz (zabrzdi)
    set_pwm2_duty(0);
    output_high(MOT_L);  // vzad
    output_high(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?
+   {
       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?
+   {
       output_low(MOT_L); // neni odraz -> vpred
       output_low(MOT_R);
       return;
    };
    SetServo((CASAVR-CASMIN));   // rovne
    set_pwm1_duty(140);  // vpred
    set_pwm2_duty(140);
    output_low(MOT_L);
    output_low(MOT_R);
    odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
+//!!! while(true) {disp(MAKE8(get_timer1(),0)); delay_ms(10);};
    while(true)
+   {
       if(!input(PROXIMITY)) break; // Je cihla blizko?
-      if(get_timer1()>=odocounter+7) return; // nedojeli jsme k cihle, jed dal
+      if(get_timer1()>(odocounter+7)) return; // nedojeli jsme k cihle, jed dal
    };
    set_pwm1_duty(0);    // reverz (zabrzdi)
    set_pwm2_duty(0);
    output_high(MOT_L);
    output_high(MOT_R);
    delay_ms(150);
    brzda();
    if (stav==cihla) while(true); // Zastav na furt, konec drahy
    if(stav==jizda)   // Objed cihlu
+   {
       #include ".\objizdka_R.c"
+   }
+}
-// Zobrazeni jednoho byte na modulu LEDbar
-void disp(int8 x)
-{
-   int n;
-   x=~x;
-   for(n=0;n<=7;n++)
-   {
-      if (x & 1 == 1) output_high(DATA); else output_low(DATA);
-      output_high(CP);
-      x>>=1;
-      output_low(CP);
-   }
-}
-// Blikani LEDbarem ve stilu Night Rider
-void NightRider(int8 x)
-{
-   int n,i,j;
-   for(j=0;j<x;j++)
-   {
-      i=0x01;
-      for(n=0;n<7;n++)
-      {
-         disp(i);
-         rotate_left(&i, 1);
-         delay_ms(40);
-      }
-      for(n=0;n<7;n++)
-      {
-         disp(i);
-         rotate_right(&i, 1);
-         delay_ms(40);
-      }
-   }
-   disp(i);
-   delay_ms(40);
-   i=0;
-   disp(i);
-}
 //---------------------------------- MAIN --------------------------------------
 void main()
+{
    int8 offset;   // Promena pro ulozeni ovsetu
    int8 rr;       // Promenna na ulozeni Rozumne rychlost
    int8 r1;       // Rychlost motoru 1
    int8 r2;       // Rychlost motoru 2
    int16 trasa;   // Pocitadlo ujete vzdalenosti
    setup_adc_ports(ALL_ANALOG);              // Zapnuti A/D prevodniku pro cteni kroutitek
    setup_adc(ADC_CLOCK_INTERNAL);
    setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro mereni casu hrany W/B v radce
    setup_timer_1(T1_EXTERNAL);               // Cita pulzy z odometrie z praveho kola
    setup_timer_2(T2_DIV_BY_16,255,1);        // Casovac PWM motoru
    setup_ccp1(CCP_PWM); // RC1               // PWM pro motory
    setup_ccp2(CCP_PWM); // RC2
    setup_comparator(NC_NC_NC_NC);
    setup_vref(FALSE);
    set_tris_c(0b11111001);     // Nastaveni vstup/vystup pro branu C, protoze se nedela automaticky
    set_pwm1_duty(0);   // Zastav motory
    set_pwm2_duty(0);
    output_low(MOT_L);
    output_low(MOT_R);
    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(0xC0);        // 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(0xC0);
    i2c_write(0x28);
    i2c_write(0b01000001);
    i2c_stop();
 /*
    i2c_start();      // Contrast (nema podstatny vliv na obraz)
    i2c_write(0xC0);
    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(0x2D);
    i2c_write(0x04 | 0x03);
    i2c_stop();
 */
    i2c_start();      // Fame Rate
    i2c_write(0xC0);
    i2c_write(0x2B);
    i2c_write(0x00);  // cca 17ms (puvodni hodnota 5Eh = 20ms)
    i2c_stop();
    i2c_start();      // VSTRT
    i2c_write(0xC0);
    i2c_write(0x19);
    i2c_write(118);   // prostredni radka
    i2c_stop();
    i2c_start();      // VEND
    i2c_write(0xC0);
    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(0xC0);
       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(0xC0);
          i2c_write(0x06);
          i2c_write(offset);  // 80h default
          i2c_stop();
          disp(offset);
          delay_ms(50);
          t1=0;
          t2=0;
          while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
          delay_ms(5);
          while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
          set_timer0(0);          // Vynuluj pocitadlo casu
          if(!input(PIX)) continue;
          while(input(PIX));
          t1=get_timer0();    // Precti cas z citace casu hrany
          set_timer0(0);          // Vynuluj pocitadlo casu
          while(!input(PIX));
          t2=get_timer0();
          if((t1>60) && (t1<140) && (t2>5) && (t2<=10)) break; // Vidis, co mas?
          delay_ms(2);   // Preskoc druhou radku z kamery
       };
       delay_ms(1000);   // Nech chvili na displayi zmerenou hodnotu
+   }
    set_adc_channel(CERVENA);   // --- Kroutitko pro jas ---
    delay_ms(1);
    offset=read_adc();
    offset &= 0b11111100;   // Dva nejnizsi bity ignoruj
 //   offset += 0x70;         // Jas nebude nikdy nizsi
    disp(offset);
    i2c_start();            // Brightness
    i2c_write(0xC0);
    i2c_write(0x06);
    i2c_write(offset);  // 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;
+   set_timer1(0);         // Vynuluj citac odometrie
-   // ......... Hlavni smycka .........
+   // ........................... Hlavni smycka ................................
    while(true)
+   {
       int8 pom;
       int8 n;
+next_snap:
       pom=0;
+      while(input(HREF));     // Preskoc 1. radku
-      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
+      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
+      {
 // !!!!Dodelat rozpoznani cerne cary napric pro zastaveni ?
          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;
          };
       };
       while(input(HREF));      // Pockej na shozeni signalu HREF
       if((pom<CASMAX) && (pom>CASMIN)) cas=pom;  // Orizni konce radku
       // Na konci obrazovaho radku to blbne. Jednak chyba od apertury
       // a vubec to nejak na kraji nefunguje.
       output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
       delay_us(1000);
       delay_us(stred);
       delay_us(stred);
       delay_us(stred);
       delay_us(cas);
       delay_us(cas);
       output_low(SERVO);
       // 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
       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);
 //      r1<<=1;     // Rychlost je dvojnasobna
 //      r2<<=1;     // Rozsah 2 az 184
       if ((stav==jizda)||(stav==cihla))      // Jizda
+      {
          set_pwm1_duty(r1);
          set_pwm2_duty(r2);
          trasa++;
+      }
       else
+      {
          set_pwm1_duty(0);       // Zastaveni
          set_pwm2_duty(0);
+      }
+//!!! trasu zrusit a predelat to na odometrii
       if((stav==jizda)&&(trasa>50)) // musi to alespon 1s jet
+      {
          ext_int_edge(H_TO_L);         // Nastav podminky preruseni od cihly
          INT0IF=0;                     // Zruseni predesle udalosti od startera
          enable_interrupts(INT_EXT);
          enable_interrupts(GLOBAL);
       };
       if(stav==start)         // Snimkuje, toci servem a ceka na start
+      {
          set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
          Delay_ms(1);
          stred=read_adc();
          if(!input(PROXIMITY))
+         {
             disp(0x80);
             while(input(PROXIMITY)); // Cekej, dokud starter neda ruku pryc
             set_timer1(0);         // Vynuluj citac odometrie
             set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
             set_pwm2_duty(255);
             disp(0x1);
             delay_ms(300);
             stav=jizda;
          };
+      }
       pom=0x80;    // Zobrazeni pozice cary na displayi
       for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
       disp(pom);
+      while(true)
+      {
+         set_adc_channel(LMAX);    // Levy UV sensor
+         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
+         if(read_adc()<128) cas=CASMIN;
+         set_adc_channel(RMAX);    // Pravy UV sensor
+         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
+         if(read_adc()<128) cas=CASMAX;
+      };
-   };
+   }
+}

Subversion Repositories svnkaklik

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