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