Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

Rev Author Line No. Line
193 kakl 1 
// **** Objeti cihly vlevo **** LLLL
2 
 
221 kakl 3 
int8 n;
4 
int8 r1,r2,rr;
5 
int1 flag;
193 kakl 6 
 
223 kakl 7 
SetServo(CASMIN-5);   // max. doleva, zatoc kolmo na caru
221 kakl 8 
set_pwm1_duty(0);
9 
set_pwm2_duty(140);
10 
output_low(MOT_L);    // vpred
193 kakl 11 
output_low(MOT_R);
223 kakl 12 
disp(0x81);
221 kakl 13 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
14 
while(get_timer1()<(odocounter+6)); // Popojed definovanou vzdalenost
193 kakl 15 
 
221 kakl 16 
set_pwm1_duty(80);
17 
set_pwm2_duty(80);
18 
set_adc_channel(LMAX);
223 kakl 19 
disp(0x82);
221 kakl 20 
cas=CASAVR-CASMIN;
21 
odocounter=get_timer1();
22 
flag=1;
23 
rr=20;
193 kakl 24 
while(true)
25 
{
221 kakl 26 
   if(input(HREF))
27 
   {
28 
      if(!input(PROXIMITY))
29 
      {
30 
         if(cas>(CASMIN+10)) cas-=10;
31 
      }
32 
      else
33 
      {
34 
         if(cas<(CASMAX-10)) cas+=10;
35 
      };
193 kakl 36 
 
221 kakl 37 
      SetServoQ(cas);
193 kakl 38 
 
221 kakl 39 
      // Elektronicky diferencial
40 
      if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
41 
      if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
42 
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
193 kakl 43 
 
221 kakl 44 
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
45 
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
193 kakl 46 
 
221 kakl 47 
      set_pwm1_duty(r1);
48 
      set_pwm2_duty(r2);
49 
 
50 
      while(input(HREF));
51 
   };
52 
   if(get_timer1()>(odocounter+5)) // Prodleva, nez se zacne detekovat cara
53 
   {
223 kakl 54 
      if(flag==1) {disp(0x83); flag=0;};
55 
      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?
221 kakl 56 
   };
57 
};
58 
 
223 kakl 59 
disp(0x84);
222 kakl 60 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
223 kakl 61 
SetServo(CASAVR);    // Primy smer
62 
while((read_adc()<THR_L) || (get_timer1()<(odocounter+2))); // Popojed za caru
221 kakl 63 
 
64 
SetServo(CASMIN);   // max. doleva                 L
223 kakl 65 
set_pwm1_duty(20);
66 
set_pwm2_duty(170);
67 
output_high(MOT_L);    // levym kolem brzdi
68 
output_low(MOT_R);
221 kakl 69 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
223 kakl 70 
while (true)  // Znovu se musime dotknout cary
71 
{
72 
   if(read_adc()<THR_L)   // Dotkli jsme se levym senzorem
73 
   {
74 
      while(read_adc()<THR_L); // Prejedeme caru
75 
      cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
76 
      break;
77 
   };
78 
   set_adc_channel(RMAX);    // Pravy UV sensor
79 
   delay_us(40);
80 
   if((get_timer1()>=(odocounter+2)) && (read_adc()<THR_L)) // Pravym senzorem nesmime caru prejet!
81 
   {
82 
      cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
83 
      break;
84 
   };
85 
   set_adc_channel(LMAX);    // Levy UV sensor
86 
   delay_us(40);
87 
}
221 kakl 88 
set_pwm1_duty(0);    // vypni motory
89 
set_pwm2_duty(0);
193 kakl 90 
 
221 kakl 91 
SetServo(CASAVR-CASMIN);   // doprostred
193 kakl 92 
 
223 kakl 93 
disp(0x85);
221 kakl 94 
set_pwm1_duty(255);    // max. vpred
95 
set_pwm2_duty(255);
193 kakl 96 
output_low(MOT_L);
97 
output_low(MOT_R);
221 kakl 98 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
99 
while(get_timer1()<(odocounter+2)) // Ujed kousek
193 kakl 100 
{
221 kakl 101 
   set_adc_channel(LMAX);    // Levy UV sensor
102 
   delay_us(40);
103 
   if(read_adc()<THR_L) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
104 
   set_adc_channel(RMAX);    // Pravy UV sensor
105 
   delay_us(40);
106 
   if(read_adc()<THR_L) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
223 kakl 107 
//   cas=CASAVR-CASMIN;    // Cara je rovne
221 kakl 108 
};