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
 // kroutitka
 #define  CERVENA  3  // AN3/RA3
 #define  CERNA    2  // AN2/RA2
 #define  ZELENA   1  // AN1/RA0
 #define  MODRA    0  // AN0/RA1
 // I/O
 #define LED    PIN_C0      // LED signalizujici start programu
 #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 DATA   PIN_B2
+#define DATA   PIN_B2      // K modulu LEDbar data
-#define CP     PIN_B1
+#define CP     PIN_B1      // K modulu LEDbar hodiny
+#define CASMIN 6           // Rozsah radku snimace
+#define CASMAX 192
+#define CASAVR ((CASMAX+CASMIN) / 2)
+// 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(int 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(100);
+      }
       for(n=0;n<7;n++)
+      {
          disp(i);
          rotate_right(&i, 1);
          delay_ms(100);
+      }
+   }
    disp(i);
    delay_ms(100);
    i=0;
    disp(i);
+}
 void main()
+{
    int8 cas;      // Cas hrany bila/cerna v radce
    int8 offset;   // Promena pro ulozeni ovsetu
    int8 rr;       // Promenna na ulozeni Rozumne rychlost
    int8 r1;       // Rychlost motoru 1
    int8 r2;       // Rychlost motoru 2
    int16 ble;     // Prodleva do rozjezdu
    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_DISABLED);
    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(0b11111000);     // 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);
    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
    i2c_write(0xC0);
    i2c_write(0x05);
    i2c_write(0xA0);  // 48h
    i2c_stop();
 */
    i2c_start();      // Brightness
    i2c_write(0xC0);
    i2c_write(0x06);
-   i2c_write(0x90);  // 80h
+   i2c_write(0x9A);  // 80h
    i2c_stop();
 /*
    i2c_start();      // Band Filter
    i2c_write(0xC0);
    i2c_write(0x2D);
    i2c_write(0x04 | 0x03);
    i2c_stop();
 */
    i2c_start();      // VSTRT
    i2c_write(0xC0);
    i2c_write(0x19);
    i2c_write(45);
    i2c_stop();
    i2c_start();      // VEND
    i2c_write(0xC0);
    i2c_write(0x1A);
    i2c_write(45);
    i2c_stop();
-   NightRider(1);    // Aby se poznalo, ze byl RESET
+   NightRider(1);    // Musi se dat cas kamere na AGC a AEC
    cas=128;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
    ble=0;
-/*
-while(true)
-{
-disp(ble);
-   i2c_start();
-   i2c_write(0xC0);
-   i2c_write(0x06);
-   i2c_write(ble);
-   i2c_stop();
-delay_ms(1000);
-ble+=0x10;
-};
-*/
    // ... Hlavni smycka ...
    while(true)
+   {
+      int8 pom;
+      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
-// Pockat na shozeni signalu HREF
-         if(!input(PIX))   // Pokud se dvakrat za sebou precetla CERNA
+         if(!input(PIX))   // Pokud se 3x za sebou precetla CERNA
          if(!input(PIX))
          if(!input(PIX))
+         {
-            int8 pom;
             pom=get_timer0();    // Precti cas z citace casu hrany
-//            cas=255-cas;         // Vlevo je vpravo
-            if(pom<0b11000000) cas=pom;
             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.
       set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
       Delay_ms(1);
       offset=read_adc();
       output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
       delay_us(1000);
       delay_us(offset);
       delay_us(offset);
       delay_us(cas);
       delay_us(cas);
       output_low(SERVO);
       ble++;                     // Casovac pro odlozeni rozjezdu po zapnuti napajeni
-      set_adc_channel(ZELENA);   // Kroutitka pro vykon motoru (!!! to bude chtit jeste predelat !!!)
+      set_adc_channel(ZELENA);   // Kroutitko pro vykon motoru
       Delay_ms(1);
-      offset=read_adc();
-      set_adc_channel(CERNA);
-      Delay_ms(1);
-      rr=read_adc();
+      rr=read_adc()>>2; //!!! // 0-31
-disp(cas);
+disp(cas & 0b11110000);
+      // 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=cas>>1;                 // Elektronicky diferencial
+//      r1<<=1;     // Rychlost je dvojnasobna
-      r2=255-offset-(cas>>1);
+//      r2<<=1;     // Rozsah 2 az 184
-      if (ble==99)               // Rozjezd na Rozumnou rychlost
-      {
-         set_pwm1_duty(rr);
-         set_pwm2_duty(rr);
-      };
       if ((ble>100)&&(ble<60000))      // Jizda
+      {
-         if (r1<=rr) set_pwm1_duty(r1);
+         set_pwm1_duty(r1);
-         if (r2<=rr) set_pwm2_duty(r2);
+         set_pwm2_duty(r2);
+      }
       else
+      {
+//!!!!!!!!!!!!!!!!!! Tenhle kod pred zavodem zrusit !!!!!!!!!!!!!!!!!!!!!!!!!!!!
          set_pwm1_duty(0);       // Zastaveni
          set_pwm2_duty(0);
+      }
    };
+}

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/camerus.c @ 360 – Rev 176 → 179