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(200);
output_low(MOT_L);    // vpred
output_low(MOT_R);
disp(0x81);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost

set_pwm1_duty(80);   // zpomal
set_pwm2_duty(80);
set_adc_channel(LMAX);
disp(0x82);
cas=CASAVR-CASMIN;         // rovne
odocounter=get_timer1();
flag=1;
rr=20;
while(true)
{
   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);

   for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
   {
/* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
      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?
      };
*/
      set_adc_channel(LMAX);
      delay_us(100);
      if(read_adc()<THR_L) {set_adc_channel(RMAX); delay_us(40); disp(0x13); goto cara1;};
      set_adc_channel(RMAX);
      delay_us(100);
      if(read_adc()<THR_L) {set_adc_channel(LMAX); delay_us(40); disp(0x23); goto cara1;};
   };
};

cara1:

//odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
//!!! SetServo(CASAVR);    // Primy smer !!!!!!!!!!!! to se nestiha
//set_pwm1_duty(140);   // zrychli
//set_pwm2_duty(140);
while(read_adc()>THR_L); // I druhym senzorem dojed k care
disp(0x84);

set_adc_channel(LMAX);
// SetServo(CASMIN);   // max. doleva                 L
set_pwm1_duty(20);
set_pwm2_duty(170);
output_high(MOT_L);    // levym kolem brzdi
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
   {
      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
         goto cara2;
      };
      set_adc_channel(RMAX);    // Pravy UV sensor
      delay_us(100);
      if((get_timer1()>=(odocounter+2)) && (read_adc()<THR_L)) // Pravym senzorem nesmime caru prejet!
      {
         cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
         goto cara2;
      };
      set_adc_channel(LMAX);    // Levy UV sensor
      delay_us(100);
   }
   SetServoQ(CASMIN);   // max. doleva                 L
}

cara2:

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