| Line 203... |
Line 203... |
| 203 |
{ |
203 |
{ |
| 204 |
olsa_pulse(); // impulz pro generovani dalsiho bitu |
204 |
olsa_pulse(); // impulz pro generovani dalsiho bitu |
| 205 |
|
205 |
|
| 206 |
if(SDOUT) // zachycena 1 |
206 |
if(SDOUT) // zachycena 1 |
| 207 |
{ |
207 |
{ |
| 208 |
bit_set(pixel,cbit); // zapise do bitu pixelu 1 - OR |
208 |
bit_set(pixel,cbit); // zapise do bitu (dano cbit) bytu (pixelu) prislusnou hodnotu |
| 209 |
} |
209 |
} |
| 210 |
} |
210 |
} |
| 211 |
olsa_pulse(); // generuje stop bit |
211 |
olsa_pulse(); // generuje stop bit |
| 212 |
if(cpixel<52) // ulozeni do pole |
212 |
if(cpixel<52) // ulozeni do pole |
| 213 |
{ |
213 |
{ |
| Line 236... |
Line 236... |
| 236 |
int8 bright; // nejsvetlejsi pixel |
236 |
int8 bright; // nejsvetlejsi pixel |
| 237 |
dark=0xff; |
237 |
dark=0xff; |
| 238 |
bright=0x00; |
238 |
bright=0x00; |
| 239 |
for(searchp=0;searchp<51;searchp++) // prohlizi levou cast radky |
239 |
for(searchp=0;searchp<51;searchp++) // prohlizi levou cast radky |
| 240 |
{ |
240 |
{ |
| 241 |
if(olsa_lseg[searchp]<dark) // porovna pixel s doposud nejtmavsim |
241 |
if(olsa_lseg[searchp]<dark) // porovna pixel s doposud nejtmavsim |
| 242 |
{ |
242 |
{ |
| 243 |
dark=olsa_lseg[searchp]; // ulozi nejtmavsi pixel |
243 |
dark=olsa_lseg[searchp]; // ulozi nejtmavsi pixel |
| 244 |
position=searchp; // ulozi polohu nejtmavsiho pixelu |
244 |
position=searchp; // ulozi polohu nejtmavsiho pixelu |
| 245 |
} |
245 |
} |
| 246 |
if(olsa_lseg[searchp]>bright) |
246 |
if(olsa_lseg[searchp]>bright) |
| 247 |
{ |
247 |
{ |
| 248 |
bright=olsa_lseg[searchp]; // ulozi nejsvetlejsi pixel |
248 |
bright=olsa_lseg[searchp]; // ulozi nejsvetlejsi pixel |
| 249 |
} |
249 |
} |
| 250 |
} |
250 |
} |
| 251 |
for(searchp=0;searchp<49;searchp++) // prohlizi levou cast radky |
251 |
for(searchp=0;searchp<49;searchp++) // prohlizi levou cast radky |
| 252 |
{ |
252 |
{ |
| 253 |
if(olsa_rseg[searchp]<dark) // porovna pixel s doposud nejtmavsim |
253 |
if(olsa_rseg[searchp]<dark) // porovna pixel s doposud nejtmavsim |
| 254 |
{ |
254 |
{ |
| 255 |
dark=olsa_rseg[searchp]; // ulozi nejtmavsi pixel |
255 |
dark=olsa_rseg[searchp]; // ulozi nejtmavsi pixel |
| 256 |
position=(searchp+51); // ulozi polohu nejtmavsiho pixelu |
256 |
position=(searchp+51); // ulozi polohu nejtmavsiho pixelu |
| 257 |
} |
257 |
} |
| 258 |
if(olsa_rseg[searchp]>bright) |
258 |
if(olsa_rseg[searchp]>bright) |
| 259 |
{ |
259 |
{ |
| 260 |
bright=olsa_rseg[searchp]; // ulozi nejsvetlejsi pixel |
260 |
bright=olsa_rseg[searchp]; // ulozi nejsvetlejsi pixel |
| 261 |
} |
261 |
} |
| 262 |
} |
262 |
} |
| 263 |
contrast=(bright-dark); |
263 |
contrast=(bright-dark); |
| 264 |
if(contrast<CONT) |
264 |
if(contrast<CONT) |
| 265 |
{ |
265 |
{ |
| Line 325... |
Line 325... |
| 325 |
void calc_regulator() |
325 |
void calc_regulator() |
| 326 |
{ |
326 |
{ |
| 327 |
int8 p_reg; |
327 |
int8 p_reg; |
| 328 |
int8 i_reg; |
328 |
int8 i_reg; |
| 329 |
int8 d_reg; |
329 |
int8 d_reg; |
| 330 |
p_reg=(CONP*err5); // vypocet proporcionalni slozky |
330 |
p_reg=(CONP*err5); // vypocet proporcionalni slozky |
| 331 |
i_reg=(CONI*(errp/100)); // vypocet integracni slozky |
331 |
i_reg=(CONI*(errp/100)); // vypocet integracni slozky |
| 332 |
if(position>old_position) // vypocet derivacni slozky |
332 |
if(position>old_position) // vypocet derivacni slozky |
| 333 |
{ |
333 |
{ |
| 334 |
d_reg=(COND*((position-old_position)/100)); // pokud je aktualni pozice vetsi nez predesla |
334 |
d_reg=(COND*((position-old_position)/100)); // pokud je aktualni pozice vetsi nez predesla |
| 335 |
} |
335 |
} |
| 336 |
else |
336 |
else |
| 337 |
{ |
337 |
{ |
| 338 |
d_reg=(COND*((old_position-position)/100)); // pokud je aktualni pozice mensi nez predesla |
338 |
d_reg=(COND*((old_position-position)/100)); // pokud je aktualni pozice mensi nez predesla |
| 339 |
} |
339 |
} |
| 340 |
reg_out=(p_reg+i_reg+d_reg); // vypocet celeho regulatoru |
340 |
reg_out=(p_reg+i_reg+d_reg); // vypocet celeho regulatoru |
| 341 |
} |
341 |
} |
| 342 |
|
342 |
|
| 343 |
// ================================== MOTORY =================================== |
343 |
// ================================== MOTORY =================================== |
| 344 |
|
344 |
|
| 345 |
void l_motor_fwd(int8 speedl) // levy motor dopredu |
345 |
void l_motor_fwd(int8 speedl) // levy motor dopredu |
| Line 531... |
Line 531... |
| 531 |
void main() |
531 |
void main() |
| 532 |
{ |
532 |
{ |
| 533 |
printf("POWER ON \r\n"); |
533 |
printf("POWER ON \r\n"); |
| 534 |
// NASTAVENI > provede se pouze pri zapnuti |
534 |
// NASTAVENI > provede se pouze pri zapnuti |
| 535 |
setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik |
535 |
setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik |
| 536 |
setup_adc_ports(ALL_ANALOG); // aktivní analogové vstupy RA0, RA1 a RA2 |
536 |
setup_adc_ports(ALL_ANALOG); // aktivni vsechny analogove vstupy |
| 537 |
setup_spi(SPI_SS_DISABLED); |
537 |
setup_spi(SPI_SS_DISABLED); |
| 538 |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
538 |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
| 539 |
setup_timer_1(T1_DISABLED); |
539 |
setup_timer_1(T1_DISABLED); |
| 540 |
setup_timer_2(T2_DIV_BY_16,255,1); // casovac pro PWM |
540 |
setup_timer_2(T2_DIV_BY_16,255,1); // casovac pro PWM |
| 541 |
setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2 |
541 |
setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2 |
| Line 614... |
Line 614... |
| 614 |
if(gap>SPACE) |
614 |
if(gap>SPACE) |
| 615 |
{ |
615 |
{ |
| 616 |
if(line_sector==LEFT) |
616 |
if(line_sector==LEFT) |
| 617 |
{ |
617 |
{ |
| 618 |
position=1; |
618 |
position=1; |
| - |
|
619 |
line_sector=LEFT; |
| 619 |
} |
620 |
} |
| 620 |
if(line_sector==RIGHT) |
621 |
if(line_sector==RIGHT) |
| 621 |
{ |
622 |
{ |
| 622 |
position=99; |
623 |
position=99; |
| - |
|
624 |
line_sector=RIGHT; |
| 623 |
} |
625 |
} |
| 624 |
} |
626 |
} |
| 625 |
if(position<20) // pro ostre zataceni |
627 |
if(position<20) // pro ostre zataceni |
| 626 |
{ |
628 |
{ |
| 627 |
position=1; |
629 |
position=1; |