Subversion Repositories svnkaklik

#include ".\camerus.h"

#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cteni z kamery

// A/D vstupy
#define  DALKOMER 4  // AN4/RA5 - dalkomer na cihlu
#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

// 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 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 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

void SetServo(int8 angle)
{
   int8 n, offset;
   
   for(n=0; n<20; n++)
   {
      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(offset);
      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);
   output_high(MOT_R);
   delay_ms(400);
   output_low(MOT_L); // zastav
   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);
      n=0;
      while(true)
      {
         while(input(ODO));
         while(!input(ODO));
         n++;
         if(n==6) break;
      }
      set_pwm1_duty(255);    // reverz (zabrzdi)
      set_pwm2_duty(255);
      output_low(MOT_L);
      output_low(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);
      output_low(MOT_L);
      output_low(MOT_R);
      stav=cihla;
      n=0;
      while(true)
      {
         while(input(ODO));
         while(!input(ODO));
         n++;
         if(n==10) break;
      }
      set_pwm1_duty(0);    // reverz (zabrzdi)
      set_pwm2_duty(0);
      output_high(MOT_L);
      output_high(MOT_R);

      SetServo((CASAVR-CASMIN));   // rovne        
      set_pwm1_duty(140);  // vpred
      set_pwm2_duty(140);
      output_low(MOT_L);
      output_low(MOT_R);
      stav=cihla;
      n=0;
      while(true)
      {
         while(input(ODO));
         while(!input(ODO));
         n++;
         if(n==10) break;
      }
      set_pwm1_duty(0);    // reverz (zabrzdi)
      set_pwm2_duty(0);
      output_high(MOT_L);
      output_high(MOT_R);
      delay_ms(200);
      set_pwm1_duty(150);  // Zastav
      set_pwm2_duty(150);
      output_high(MOT_L);
      output_high(MOT_R);
      delay_ms(1000);

      SetServo(CASMIN);   // max. doleva      
      set_pwm1_duty(0);  // vzad
      set_pwm2_duty(20);
      output_low(MOT_L);
      output_high(MOT_R);
      stav=cihla;
      n=0;
      while(true)
      {
         while(input(ODO));
         while(!input(ODO));
         n++;
         if(n==10) break;
      }
      set_pwm1_duty(0);    // reverz (zabrzdi)
      set_pwm2_duty(255);
      output_low(MOT_L);
      output_low(MOT_R);
      delay_ms(100);
      set_pwm1_duty(0);  // Zastav
      set_pwm2_duty(0);
      output_low(MOT_L);
      output_low(MOT_R);
      delay_ms(500);


      SetServo((CASAVR-CASMIN)+5);   // mirne doprava      
      set_pwm1_duty(180);  // vpred
      set_pwm2_duty(180);
      output_low(MOT_L);
      output_low(MOT_R);
      stav=cihla;
      n=0;
      while(true)
      {
         while(input(ODO));
         while(!input(ODO));
         n++;
         if(n==10) break;
      }
      set_pwm1_duty(0);  // Zastav
      set_pwm2_duty(0);
      output_low(MOT_L);
      output_low(MOT_R);
      delay_ms(500);


      cas=CASMIN;  // Cara je vlevo
      
      stav=cihla;
   }
}

// 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);
}


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_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(0x0);  // 80h //9Ah
   i2c_stop();

   i2c_start();      // Band Filter
   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

{ // Kalibrace kamery
   int8 t1,t2;

   for(offset=0;offset<=255;offset+=0x04) // Cita porad dokola
   {
      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;

      delay_ms(2);
   }
}

delay_ms(1000);

   cas=CASAVR-CASMIN;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
   stav=start;
   trasa=0;

   // ... Hlavni smycka ...
   while(true)
   {
      int8 pom;
      int8 n;

      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
         if(!input(PIX))   // Pokud se Xx 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.

      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(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

      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);
      }

      set_adc_channel(DALKOMER);   // Prepni A/D prevodnik na detektor cihly
      Delay_ms(1);

      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)
      if(read_adc()<128)
      {
         disp(0x55);
         while(read_adc()<128); // Cekej, dokud starter neda ruku pryc
         set_pwm1_duty(255);
         set_pwm2_duty(255);
         disp(0xAA);
         delay_ms(200);
         stav=jizda;
      }

      pom=0x80;
      for(n=CASMAX/8; n<cas; n+=CASMAX/8)
      {
         pom>>=1;
      }
      disp(pom);

   output_high(PIN_C0);
   delay_ms(1);
   output_low(PIN_C0);

   };
}