| 0,0 → 1,64 |
|---|
| void objizdka() |
| { |
| int8 shure=0; |
| unsigned int16 n; |
| |
| // toceni na miste dokud nezmizi cihla |
| //------------------------------------ |
| uhel=KOLMO1; // nastav zataceci kolecko kolmo na osu robota |
| Delay_ms(100); |
| BL;FR; |
| Delay_ms(200); // minimalni toceni, kdyby se zastavilo sikmo k cihle |
| |
| While(bit_test(sensors,7)) // toc, dokud neni cihla z primeho senzoru |
| { |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| Delay_ms(4); // cekani na SLAVE nez pripravi data od cidel |
| } |
| STOPL; STOPR; |
| |
| for (n=0;n<1000;n++) // vystred se na hranu cihly |
| { |
| if(!input(CIHLA)) |
| { |
| GO(L,B,180);GO(R,F,160); // zapni motory PWM podle promenych Lmotor a Rmotor |
| } else |
| { |
| GO(L,F,180);GO(R,B,160); // zapni motory PWM podle promenych Lmotor a Rmotor |
| }; |
| delay_ms(1); |
| } |
| STOPR;STOPL; |
| |
| uhel=STRED; // dopredu |
| delay_ms(100); |
| FR; FL; |
| delay_ms(500); |
| BL;BR; |
| delay_ms(200); |
| STOPL;STOPR; |
| |
| uhel=STRED+BEAR3; // doprava |
| delay_ms(100); |
| FL; |
| delay_ms(400); |
| uhel=STRED+BEAR2; // min doprava |
| FL;FR; |
| delay_ms(100); |
| uhel=STRED+BEAR1; // jeste min doprava |
| FL;FR; |
| delay_ms(200); |
| While((sensors & 0b11111110)!=0) //dokud neni cara |
| { |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| Delay_ms(4); // cekani na SLAVE nez pripravi data od cidel |
| } |
| BL; BR; |
| delay_ms(400); |
| |
| uhel=STRED-BEAR3; // doleva |
| } |
| |
| //////////////////////////////////////////////////////////////////////////////// |