Subversion Repositories svnkaklik

Rev

Rev 226 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log

Rev 226 Rev 227
Line 8... Line 8...
8 
int8 r1,r2,rr;
 8 
int8 r1,r2,rr;
9 
int8 touch;
 9 
int8 touch;
10 
enum okolo_cihly {pred_carou,na_care,po_care};
 10 
enum okolo_cihly {pred_carou,na_care,po_care};
11 
okolo_cihly ridic;       // V jakem jsme stavu objizdeni cihly
 11 
okolo_cihly ridic;       // V jakem jsme stavu objizdeni cihly
12 
 
 12 
 
13 
rr=20;
 13 
cihla:
14 
 
 14 
 
- 
 
 15 
   rr=20;
15 
disp(0x99);
 16 
   disp(0x99);
- 
 
 17 
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vpred
- 
 
 18 
   set_pwm2_duty(150);
- 
 
 19 
   output_high(MOT_L);
- 
 
 20 
   output_low(MOT_R);
16 
odocounter=get_timer1();
 21 
   odocounter=get_timer1();
17 
while(true) // Na zacatku se vyhni cihle, zatoc co muzes
 22 
   while(true) // Na zacatku se vyhni cihle, zatoc co muzes
18 
{
 23 
   {
19 
   cas=CASMIN-5;       // jeste vic nez hodne do leva
 24 
      cas=CASMIN-5;       // jeste vic nez hodne do leva
20 
 
 25
21 
   set_pwm1_duty(0);
 26 
      set_pwm1_duty(0);
22 
   set_pwm2_duty(200);
 27 
      set_pwm2_duty(200);
23 
   output_high(MOT_L);     // leve kolo reverz
 28 
      output_high(MOT_L);     // leve kolo reverz
24 
   output_low(MOT_R);      // prave kolo vpred
 29 
      output_low(MOT_R);      // prave kolo vpred
25 
   if(get_timer1()>(odocounter+5))  // konec zatacky?
 30 
      if(get_timer1()>(odocounter+8))  // konec zatacky?
26 
   {
 31 
      {
27 
      disp(0x66);
 32 
         disp(0x66);
28 
      cas=CASAVR-CASMIN;   // rovne
 33 
         cas=CASAVR-CASMIN;   // rovne
29 
      break;
 34 
         break;
30 
   }
 35 
      }
31 
   SetServoQ(cas);
 36 
      SetServoQ(cas);
32 
   delay_ms(18);
 37 
      delay_ms(18);
33 
};
 38 
   };
34 
 
 39
- 
 
 40 
   //------ Objeti cihly v konstantni vzdalenosti ------
35 
touch=0;
 41 
   touch=0;
36 
ridic=pred_carou;
 42 
   ridic=pred_carou;
37 
output_low(MOT_L);   // vpred
 43 
   output_low(MOT_L);   // vpred
38 
output_low(MOT_R);
 44 
   output_low(MOT_R);
39 
while(true)
 45 
   while(true)
40 
{
 46 
   {
- 
 
 47 
      if(!input(IRRX)) goto cihla;
41 
 
 48
42 
   if(!input(PROXIMITY))   // Udrzovani konstantni vzdalenosti od cihly
 49 
      if(!input(PROXIMITY))   // Udrzovani konstantni vzdalenosti od cihly
43 
   {
 50 
      {
44 
      if(cas>(CASMIN+20)) cas-=20;
 51 
         if(cas>(CASMIN+20)) cas-=20;
45 
   }
 52 
      }
Line 76... Line 83...
76 
   if((ridic==na_care)&&(touch==0)) break;
 83 
      if((ridic==na_care)&&(touch==0)) break;
77 
   if(ridic==na_care) touch=0;
 84 
      if(ridic==na_care) touch=0;
78 
};
 85 
   };
79 
disp(0xC3);
 86 
   disp(0xC3);
80 
 
 87
81 
set_pwm1_duty(0);
 88 
   set_pwm1_duty(20);
82 
set_pwm2_duty(170);
 89 
   set_pwm2_duty(200);
83 
output_low(MOT_L);
 90 
   output_high(MOT_L);
84 
output_low(MOT_R);
 91 
   output_low(MOT_R);
85 
delay_us(40);
 92 
   delay_us(40);
86 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 93 
   odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
87 
while (true)  // Znovu se musime dotknout cary
 94 
   while (true)  // Znovu se musime dotknout cary
88 
{
 95 
   {
Line 103... Line 110...
103 
         disp(0x07);
 110 
            disp(0x07);
104 
         cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
 111 
            cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
105 
         goto cara;
 112 
            goto cara;
106 
      };
 113 
         };
107 
   }
 114 
      }
108 
   SetServoQ(CASMIN);   // max. doleva                 L
 115 
      SetServoQ(CASMIN-5);   // max. max. doleva                 L
109 
}
 116 
   }
110 
 
 117
111 
cara:
 118 
cara:
112 
;
 119
- 
 
 120 
   output_low(MOT_L); // oba motory vpred
- 
 
 121 
   output_low(MOT_R);
113 
/*
 122 
/*
114 
set_pwm1_duty(0);    // vypni motory
 123 
set_pwm1_duty(0);    // vypni motory
115 
set_pwm2_duty(0);
 124 
set_pwm2_duty(0);
116 
 
 125 
 
117 
SetServo(CASAVR-CASMIN);   // doprostred
 126 
SetServo(CASAVR-CASMIN);   // doprostred