Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

Rev Author Line No. Line
2 kaklik 1 
void objizdka()
2 
{
3 
   int8 shure=0;
4 
   unsigned int16 n;
5 
 
6 
// toceni na miste dokud nezmizi cihla
7 
//------------------------------------
8 
   uhel=KOLMO1;      // nastav zataceci kolecko kolmo na osu robota
9 
   Delay_ms(100);
10 
   BL;FR;
11 
   Delay_ms(200);    // minimalni toceni, kdyby se zastavilo sikmo k cihle
12 
 
13 
   While(bit_test(sensors,7)) // toc, dokud neni cihla z primeho senzoru
14 
   {
15 
       sensors = spi_read(0);         // cteni senzoru
16 
       sensors=~sensors;
17 
       Delay_ms(4);              // cekani na SLAVE nez pripravi data od cidel
18 
   }
19 
   STOPL; STOPR;
20 
 
21 
   for (n=0;n<1000;n++)    // vystred se na hranu cihly
22 
   {
23 
      if(!input(CIHLA))
24 
      {
25 
         GO(L,B,180);GO(R,F,160);   // zapni motory PWM podle promenych Lmotor a Rmotor
26 
      } else
27 
      {
28 
         GO(L,F,180);GO(R,B,160);   // zapni motory PWM podle promenych Lmotor a Rmotor
29 
      };
30 
      delay_ms(1);
31 
   }
32 
   STOPR;STOPL;
33 
 
34 
   uhel=STRED;          // dopredu
35 
   delay_ms(100);
36 
   FR; FL;
37 
   delay_ms(500);
38 
   BL;BR;
39 
   delay_ms(200);
40 
   STOPL;STOPR;
41 
 
42 
   uhel=STRED+BEAR3;    // doprava
43 
   delay_ms(100);
44 
   FL;
45 
   delay_ms(400);
46 
   uhel=STRED+BEAR2;    // min doprava
47 
   FL;FR;
48 
   delay_ms(100);
49 
   uhel=STRED+BEAR1;    // jeste min doprava
50 
   FL;FR;
51 
   delay_ms(200);
52 
   While((sensors & 0b11111110)!=0) //dokud neni cara
53 
   {
54 
       sensors = spi_read(0);         // cteni senzoru
55 
       sensors=~sensors;
56 
       Delay_ms(4);              // cekani na SLAVE nez pripravi data od cidel
57 
   }
58 
   BL; BR;
59 
   delay_ms(400);
60 
 
61 
   uhel=STRED-BEAR3;    // doleva
62 
}
63 
 
64 
////////////////////////////////////////////////////////////////////////////////