Subversion Repositories svnkaklik

// **** Objeti cihly vlevo **** LLLL

#define L_TOUCH 1    // Cara vlevo
#define R_TOUCH 2    // Cata vpravo
#define B_TOUCH 3    // Both

int8 n;
int8 r1,r2,rr;
int8 touch;
enum okolo_cihly {pred_carou,na_care,po_care};
okolo_cihly ridic;       // V jakem jsme stavu objizdeni cihly
int8 vzdalenost;

   stav=cihla; // Dalsi prekazku uz nezaznamenavej (je to s velkou pravdepodobnosti cil)
   touch=0;    // Indikator detekce cary pri objizdeni

cihla:

   rr=RR_CIHLA;   //!!! Rozumna rychlost pro objizdeni cihly (bylo by lepsi rychlost zvysovat) a pri detekci pohybu zase snizit
   disp(0x99);
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vpred
   set_pwm2_duty(200);
   output_high(MOT_L);
   output_low(MOT_R);
   odocounter=get_timer1();
   while(true) // Na zacatku se vyhni cihle, zatoc co muzes
   {
      cas=CASMIN-5;       // jeste vic nez hodne do leva

      if(BUMPER) // Narazili jsme do cihly, musime couvnout!
      {
         bum();
         SaveLog(log-1);      // Zapis Black Boxu do EEPROM
         brzda();
         goto cihla; // Znovu zacni cihlu objizdet
      };

      set_pwm1_duty(0);
      set_pwm2_duty(200);
      output_high(MOT_L);     // leve kolo reverz
      output_low(MOT_R);      // prave kolo vpred
      if(get_timer1()>(odocounter+5))  // konec zatacky?
      {
         disp(0x66);
         break;
      }
      SetServoQ(cas);
      delay_ms(18);
   };

   //------ Objeti cihly v konstantni vzdalenosti ------
   ridic=pred_carou;
   cas=CASAVR-CASMIN;   // rovne
   output_low(MOT_L);   // vpred
   output_low(MOT_R);
   while(true)
   {
      if(BUMPER) // Narazili jsme do cihly, musime couvnout!
      {
         bum();
         SaveLog(log-1);      // Zapis Black Boxu do EEPROM
         set_pwm1_duty(140);  // vpred
         set_pwm2_duty(140);
         output_low(MOT_L);
         output_low(MOT_R);
      };

      delta_bearing=bearing-bearing_offset;
      if(IRRX || ((delta_bearing>64)&&(delta_bearing<128))) // hrozi celni srazka s cihlou v prubehu objizdeni
      {
         cas=CASMIN;
      }
      else
      {
         if((vzdalenost!=0)||!input(PROXIMITY)) // Udrzovani konstantni vzdalenosti od cihly
         {
            if(cas>(CASMIN+20)) cas-=20;
         }
         else
         {
            if(cas<(CASMAX-20)) cas+=20;
         };
      };
      // 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);

      set_pwm1_duty(r1);   // Nastav rychlost motoru
      set_pwm2_duty(r2);

      SetServoQ(cas);

      i2c_start();     // Sonar Ping
      i2c_write(SONAR_ADR);
      i2c_write(0x0);
      i2c_write(0x52);  // mereni v us
      i2c_stop();

      for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
      {
         set_adc_channel(LMAX);
         delay_us(100);
         if(read_adc()<THR) touch|=L_TOUCH;
         set_adc_channel(RMAX);
         delay_us(100);
         if(read_adc()<THR) touch|=R_TOUCH;
      };

      i2c_start();     // Odraz ze sonaru
      i2c_write(SONAR_ADR);
      i2c_write(0x3);
      i2c_stop();
      i2c_start();
      i2c_write(SONAR_ADR+1);
      vzdalenost=i2c_read(0);
      i2c_stop();

      i2c_start();     // Cteni kompasu
      i2c_write(COMPAS_ADR);
      i2c_write(0x1);   // uhel 0-255
      i2c_stop();
      i2c_start();
      i2c_write(COMPAS_ADR+1);
      bearing=i2c_read(0);
      i2c_stop();

      if(touch==L_TOUCH) disp(0xC0);
      if(touch==R_TOUCH) disp(0x03);
      if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care;
      if((ridic==na_care)&&(touch==0)) break;
      if(ridic==na_care) touch=0;
   };
   disp(0xC3);

   set_pwm1_duty(20);
   set_pwm2_duty(200);
   output_high(MOT_L);
   output_low(MOT_R);
   delay_us(40);
   odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
   while (true)  // Znovu se musime dotknout cary
   {
      for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
      {
         set_adc_channel(LMAX);    // Levy UV sensor
         delay_us(100);
         if(read_adc()<THR)   // Dotkli jsme se levym senzorem
         {
            disp(0xE0);
            cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
            goto cara;
         };
         set_adc_channel(RMAX);    // Pravy UV sensor
         delay_us(100);
         if((get_timer1()>=(odocounter+2)) && (read_adc()<THR)) // Pravym senzorem nesmime caru prejet!
         {
            disp(0x07);
            cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
            goto cara;
         };
      }
      SetServoQ(CASMIN-5);   // max. max. doleva                 L
   }

cara:

   output_low(MOT_L); // oba motory vpred
   output_low(MOT_R);