Subversion Repositories svnkaklik

Rev

Rev 193 | Go to most recent revision | Details | Compare with Previous | 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 
 
221 kakl 7 
SetServo(CASMIN-3);   // max. doleva, zatoc kolmo na caru
8 
set_pwm1_duty(0);
9 
set_pwm2_duty(140);
10 
output_low(MOT_L);    // vpred
193 kakl 11 
output_low(MOT_R);
221 kakl 12 
disp(1);
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);
19 
disp(2);
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 
   {
54 
      if(flag==1) {disp(3); 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;};};
56 
   };
57 
};
58 
 
59 
disp(4);
60 
while (read_adc()<THR_L);
61 
 
62 
SetServo(CASMIN);   // max. doleva                 L
63 
set_pwm1_duty(0);
193 kakl 64 
set_pwm2_duty(140);
221 kakl 65 
while (read_adc()>THR_L);
66 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
67 
while(get_timer1()<(odocounter+3)); // Popojed definovanou vzdalenost
68 
set_pwm1_duty(0);    // vypni motory
69 
set_pwm2_duty(0);
70 
/*
193 kakl 71 
set_pwm1_duty(0);    // reverz (zabrzdi)
72 
set_pwm2_duty(0);
73 
output_low(MOT_L);
74 
output_high(MOT_R);
221 kakl 75 
*/
193 kakl 76 
 
221 kakl 77 
SetServo(CASAVR-CASMIN);   // doprostred
193 kakl 78 
 
221 kakl 79 
disp(5);
80 
set_pwm1_duty(255);    // max. vpred
81 
set_pwm2_duty(255);
193 kakl 82 
output_low(MOT_L);
83 
output_low(MOT_R);
221 kakl 84 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
85 
while(get_timer1()<(odocounter+2)) // Ujed kousek
193 kakl 86 
{
221 kakl 87 
   set_adc_channel(LMAX);    // Levy UV sensor
88 
   delay_us(40);
89 
   if(read_adc()<THR_L) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
90 
   set_adc_channel(RMAX);    // Pravy UV sensor
91 
   delay_us(40);
92 
   if(read_adc()<THR_L) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
93 
   cas=CASAVR-CASMIN;    // Cara je rovne
94 
};