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
////////////////////////////////////////////////////////////////////////////////