Subversion Repositories svnkaklik

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

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

SetServo(CASMIN-5);   // max. doleva, zatoc kolmo na caru
set_pwm1_duty(0);
set_pwm2_duty(140);
output_low(MOT_L);    // vpred
output_low(MOT_R);
disp(0x81);
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(0x82);
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(0x83); flag=0;};
      if(read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) break;};}; // Neni to jen skvira nebo spina?
   };
};

disp(0x84);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
SetServo(CASAVR);    // Primy smer
while((read_adc()<THR_L) || (get_timer1()<(odocounter+2))); // Popojed za caru

SetServo(CASMIN);   // max. doleva                 L
set_pwm1_duty(20);
set_pwm2_duty(170);
output_high(MOT_L);    // levym kolem brzdi
output_low(MOT_R);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while (true)  // Znovu se musime dotknout cary
{
   if(read_adc()<THR_L)   // Dotkli jsme se levym senzorem
   {
      while(read_adc()<THR_L); // Prejedeme caru
      cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
      break;
   };
   set_adc_channel(RMAX);    // Pravy UV sensor
   delay_us(40);
   if((get_timer1()>=(odocounter+2)) && (read_adc()<THR_L)) // Pravym senzorem nesmime caru prejet!
   {
      cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
      break;
   };
   set_adc_channel(LMAX);    // Levy UV sensor
   delay_us(40);
}
set_pwm1_duty(0);    // vypni motory
set_pwm2_duty(0);

SetServo(CASAVR-CASMIN);   // doprostred

disp(0x85);
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
};