Subversion Repositories svnkaklik

//******** Robot Camerus pro IstRobot 2007 ************

//"$Id: camerus.c 192 2007-03-16 22:56:25Z 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  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

// 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 IRRX   PIN_B0      // Vstup INT, generuje preruseni pri prekazce

#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)

#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

inline 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<150;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               // PWM pro motory

   setup_ccp2(CCP_PWM); // RC2

}

void SetServo(int8 angle)

{

   int8 n, offset;

   for(n=0; n<14; 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);  // 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

   delay_ms(200);    // Doba reverzace

   brzda();

   if (stav==cihla) while(true); // Zastav na furt, konec drahy

   if(stav==jizda)   // Objed cihlu

   {

      int n;

      SetServo(CASMIN);   // prudce doleva

      set_pwm1_duty(0);  // vpred

      set_pwm2_duty(200);

      output_low(MOT_L);

      output_low(MOT_R);

      n=0;

      while(true)

      {

         while(input(ODO));

         while(!input(ODO));

         n++;

         if(n==7) break;

      }

      set_pwm1_duty(0);    // reverz (zabrzdi)

      set_pwm2_duty(0);

      output_low(MOT_L);

      output_high(MOT_R);

      delay_ms(200);

      brzda();

      SetServo((CASAVR-CASMIN));   // rovne

      set_pwm1_duty(140);  // vpred

      set_pwm2_duty(140);

      output_low(MOT_L);

      output_low(MOT_R);

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

      brzda();

      SetServo(CASMIN);   // max. doleva

      set_pwm1_duty(0);   // vzad

      set_pwm2_duty(20);

      output_low(MOT_L);

      output_high(MOT_R);

      n=0;

      while(true)

      {

         while(input(ODO));

         while(!input(ODO));

         n++;

         if(n==9) break;

      }

      set_pwm1_duty(0);    // reverz (zabrzdi)

      set_pwm2_duty(255);

      output_low(MOT_L);

      output_low(MOT_R);

      delay_ms(200);

      brzda();

      SetServo((CASAVR-CASMIN));   // rovne

      set_pwm1_duty(140);  // vpred

      set_pwm2_duty(140);

      output_low(MOT_L);

      output_low(MOT_R);

      n=0;

      while(true)

      {

         while(input(ODO));

         while(!input(ODO));

         n++;

         if(n==13) break;

      }

      set_pwm1_duty(0);    // reverz (zabrzdi)

      set_pwm2_duty(0);

      output_high(MOT_L);

      output_high(MOT_R);

      delay_ms(200);

      brzda();

      SetServo(CASMIN);   // max. doleva

      set_pwm1_duty(0);   // vzad

      set_pwm2_duty(20);

      output_low(MOT_L);

      output_high(MOT_R);

      n=0;

      while(true)

      {

         while(input(ODO));

         while(!input(ODO));

         n++;

         if(n==6) break;

      }

      set_pwm1_duty(0);    // reverz (zabrzdi)

      set_pwm2_duty(255);

      output_low(MOT_L);

      output_low(MOT_R);

      delay_ms(200);

      brzda();

      SetServo((CASAVR-CASMIN));   // rovne

      set_pwm1_duty(190);  // vpred

      set_pwm2_duty(190);

      output_low(MOT_L);

      output_low(MOT_R);

      n=0;

      while(true)

      {

         while(input(ODO));

         while(!input(ODO));

         n++;

         if(n==9) break;

      }

/*

      set_pwm1_duty(0);    // reverz (zabrzdi)

      set_pwm2_duty(0);

      output_high(MOT_L);

      output_high(MOT_R);

      delay_ms(100);

*/

      brzda();

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

   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=0x60;offset<(255-0x04);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;

         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()>>1;   // 0-127

   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;

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

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

      // 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)         // Snimkuje, toci servem a ceka na start

      if(read_adc()<128)

      {

         disp(0x80);

         while(read_adc()<128); // Cekej, dokud starter neda ruku pryc

         set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu

         set_pwm2_duty(255);

         disp(0x1);

         delay_ms(400);

         stav=jizda;

      }

      pom=0x80;    // Zobrazeni pozice cary na displayi

      for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;

      disp(pom);

   };

}