Subversion Repositories svnkaklik

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

int8 n;
int8 r1,r2,rr;
int1 flag;

SetServo(CASMIN-3);   // max. doleva, zatoc kolmo na caru
set_pwm1_duty(0);
set_pwm2_duty(140);
output_low(MOT_L);    // vpred
output_low(MOT_R);
disp(1);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+6)); // Popojed definovanou vzdalenost

set_pwm1_duty(80);
set_pwm2_duty(80);
set_adc_channel(LMAX);
disp(2);
cas=CASAVR-CASMIN;
odocounter=get_timer1();
flag=1;
rr=20;
while(true)
{
   if(input(HREF))
   {
      if(!input(PROXIMITY))
      {
         if(cas>(CASMIN+10)) cas-=10;
      }
      else
      {
         if(cas<(CASMAX-10)) cas+=10;
      };

      SetServoQ(cas);

      // 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);
      set_pwm2_duty(r2);

      while(input(HREF));
   };
   if(get_timer1()>(odocounter+5)) // Prodleva, nez se zacne detekovat cara
   {
      if(flag==1) {disp(3); flag=0;};
      if(read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) break;};};
   };
};

disp(4);
while(read_adc()<THR_L);   // Prejed caru
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while((read_adc()<THR_L) || (get_timer1()<(odocounter+1))); // Popojed za caru

SetServo(CASMIN);   // max. doleva                 L
set_pwm1_duty(0);
set_pwm2_duty(140);
while (read_adc()>THR_L);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+3)); // Popojed definovanou vzdalenost
set_pwm1_duty(0);    // vypni motory
set_pwm2_duty(0);
/*
set_pwm1_duty(0);    // reverz (zabrzdi)
set_pwm2_duty(0);
output_low(MOT_L);
output_high(MOT_R);
*/

SetServo(CASAVR-CASMIN);   // doprostred

disp(5);
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_L) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
   set_adc_channel(RMAX);    // Pravy UV sensor
   delay_us(40);
   if(read_adc()<THR_L) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
   cas=CASAVR-CASMIN;    // Cara je rovne
};