Line 230... |
Line 230... |
230 |
} |
230 |
} |
231 |
|
231 |
|
232 |
void olsa_position() // vyhodnoti pozici cary |
232 |
void olsa_position() // vyhodnoti pozici cary |
233 |
{ |
233 |
{ |
234 |
int8 searchp; // ukazatel na pole |
234 |
int8 searchp; // ukazatel na pole |
235 |
int8 search; // ulozeni prectene hodnoty |
- |
|
236 |
int8 dark; // nejtmavsi pixel |
235 |
int8 dark; // nejtmavsi pixel |
237 |
int8 bright; // nejsvetlejsi pixel |
236 |
int8 bright; // nejsvetlejsi pixel |
238 |
dark=0xff; |
237 |
dark=0xff; |
239 |
bright=0x00; |
238 |
bright=0x00; |
240 |
for(searchp=0;searchp<52;searchp++) // prohlizi levou cast radky |
239 |
for(searchp=0;searchp<51;searchp++) // prohlizi levou cast radky |
241 |
{ |
240 |
{ |
242 |
search=olsa_lseg[searchp]; // vybira pixel |
- |
|
243 |
if((search<dark)&&(search>0x00)) // porovna pixel s doposud nejtmavsim |
241 |
if(olsa_lseg[searchp]<dark) // porovna pixel s doposud nejtmavsim |
244 |
{ |
242 |
{ |
245 |
dark=search; // ulozi nejtmavsi pixel |
243 |
dark=olsa_lseg[searchp]; // ulozi nejtmavsi pixel |
246 |
position=searchp; // ulozi polohu nejtmavsiho pixelu |
244 |
position=searchp; // ulozi polohu nejtmavsiho pixelu |
247 |
} |
245 |
} |
248 |
if((search>bright)&&(search<0xff)) |
246 |
if(olsa_lseg[searchp]>bright) |
249 |
{ |
247 |
{ |
250 |
bright=search; // ulozi nejsvetlejsi pixel |
248 |
bright=olsa_lseg[searchp]; // ulozi nejsvetlejsi pixel |
251 |
} |
249 |
} |
252 |
} |
250 |
} |
253 |
for(searchp=0;searchp<52;searchp++) // prohlizi pravou cast radky |
251 |
for(searchp=0;searchp<49;searchp++) // prohlizi levou cast radky |
254 |
{ |
252 |
{ |
255 |
search=olsa_rseg[searchp]; // vybira pixel |
- |
|
256 |
if((search<dark)&&(search>0x00)) // porovna pixel s doposud nejtmavsim |
253 |
if(olsa_rseg[searchp]<dark) // porovna pixel s doposud nejtmavsim |
257 |
{ |
254 |
{ |
258 |
dark=search; // ulozi nejtmavsi pixel |
255 |
dark=olsa_rseg[searchp]; // ulozi nejtmavsi pixel |
259 |
position=(searchp+50); // ulozi polohu nejtmavsiho pixelu |
256 |
position=(searchp+51); // ulozi polohu nejtmavsiho pixelu |
260 |
} |
257 |
} |
261 |
if((search>bright)&&(search<0xff)) |
258 |
if(olsa_rseg[searchp]>bright) |
262 |
{ |
259 |
{ |
263 |
bright=search; // ulozi nejsvetlejsi pixel |
260 |
bright=olsa_rseg[searchp]; // ulozi nejsvetlejsi pixel |
264 |
} |
261 |
} |
265 |
} |
262 |
} |
266 |
contrast=(bright-dark); |
263 |
contrast=(bright-dark); |
267 |
if(contrast<CONT) |
264 |
if(contrast<CONT) |
268 |
{ |
265 |
{ |
Line 461... |
Line 458... |
461 |
{ |
458 |
{ |
462 |
l_motor_bwd(255); // zatoc doleva |
459 |
l_motor_bwd(255); // zatoc doleva |
463 |
r_motor_fwd(255); |
460 |
r_motor_fwd(255); |
464 |
delay_ms(350); |
461 |
delay_ms(350); |
465 |
l_motor_fwd(255); // jed rovne |
462 |
l_motor_fwd(255); // jed rovne |
466 |
delay_ms(1000); |
463 |
delay_ms(800); |
467 |
r_motor_bwd(255); // zatoc doprava |
464 |
r_motor_bwd(255); // zatoc doprava |
468 |
delay_ms(350); |
465 |
delay_ms(300); |
469 |
r_motor_fwd(255); // jed rovne |
466 |
r_motor_fwd(255); // jed rovne |
470 |
delay_ms(1000); |
467 |
delay_ms(1200); |
471 |
r_motor_bwd(255); // zatoc doprava |
468 |
r_motor_bwd(255); // zatoc doprava |
472 |
delay_ms(300); |
469 |
delay_ms(200); |
473 |
r_motor_fwd(255); // jed rovne |
470 |
r_motor_fwd(255); // jed rovne |
474 |
delay_ms(800); |
471 |
delay_ms(200); |
475 |
position=40; |
472 |
position=40; |
476 |
} |
473 |
} |
477 |
// ================================ DIAGNOSTIKA ================================ |
474 |
// ================================ DIAGNOSTIKA ================================ |
478 |
|
475 |
|
479 |
void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru |
476 |
void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru |
Line 622... |
Line 619... |
622 |
if(line_sector==RIGHT) |
619 |
if(line_sector==RIGHT) |
623 |
{ |
620 |
{ |
624 |
position=99; |
621 |
position=99; |
625 |
} |
622 |
} |
626 |
} |
623 |
} |
- |
|
624 |
if(position<20) // pro ostre zataceni |
- |
|
625 |
{ |
- |
|
626 |
position=1; |
- |
|
627 |
} |
- |
|
628 |
if(position>80) // pro ostre zataceni |
- |
|
629 |
{ |
- |
|
630 |
position=100; |
- |
|
631 |
} |
627 |
calc_error(); |
632 |
calc_error(); |
628 |
calc_regulator(); |
633 |
calc_regulator(); |
629 |
//printf("regulator: %u\r\n",reg_out); |
634 |
//printf("regulator: %u\r\n",reg_out); |
630 |
if(position<50) // prepocet regulatoru pro motory, pokud je cara vlevo |
635 |
if(position<50) // prepocet regulatoru pro motory, pokud je cara vlevo |
631 |
{ |
636 |
{ |