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

rr=20;

disp(0x99);
odocounter=get_timer1();
while(true) // Na zacatku se vyhni cihle, zatoc co muzes
{
   cas=CASMIN-5;       // jeste vic nez hodne do leva

   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);
      cas=CASAVR-CASMIN;   // rovne
      break;
   }
   SetServoQ(cas);
   delay_ms(18);
};

touch=0;
ridic=pred_carou;
output_low(MOT_L);   // vpred
output_low(MOT_R);
while(true)
{

   if(!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);

   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;
   };
   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(0);
set_pwm2_duty(170);
output_low(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);   // max. doleva                 L
}

cara:
;
/*
set_pwm1_duty(0);    // vypni motory
set_pwm2_duty(0);

SetServo(CASAVR-CASMIN);   // doprostred

set_pwm1_duty(255);    // max. vpred
set_pwm2_duty(255);
output_low(MOT_L);
output_low(MOT_R);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+2)) // Ujed kousek
{
   set_adc_channel(LMAX);    // Levy UV sensor
   delay_us(40);
   if(read_adc()<THR) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
   set_adc_channel(RMAX);    // Pravy UV sensor
   delay_us(40);
   if(read_adc()<THR) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
};
*/