10,6 → 10,7 |
enum okolo_cihly {pred_carou,na_care,po_care}; |
okolo_cihly ridic; // V jakem jsme stavu objizdeni cihly |
int8 vzdalenost; |
int8 visualisation; |
|
stav=cihla; // Dalsi prekazku uz nezaznamenavej (je to s velkou pravdepodobnosti cil) |
touch=0; // Indikator detekce cary pri objizdeni |
53,6 → 54,7 |
cas=CASAVR-CASMIN; // rovne |
output_low(MOT_L); // vpred |
output_low(MOT_R); |
visualisation=0; |
while(true) |
{ |
if(BUMPER) // Narazili jsme do cihly, musime couvnout! |
59,20 → 61,21 |
{ |
bum(); |
SaveLog(log-1); // Zapis Black Boxu do EEPROM |
set_pwm1_duty(140); // vpred |
set_pwm2_duty(140); |
set_pwm1_duty(160); // vpred |
set_pwm2_duty(160); |
output_low(MOT_L); |
output_low(MOT_R); |
}; |
|
delta_bearing=bearing-bearing_offset; |
if(IRRX || ((delta_bearing>64)&&(delta_bearing<128))) // hrozi celni srazka s cihlou v prubehu objizdeni |
visualisation=(delta_bearing & 0xF0) | (visualisation & 0x0F); |
if(IRRX) // hrozi celni srazka s cihlou v prubehu objizdeni |
{ |
cas=CASMIN; |
} |
else |
{ |
if((vzdalenost!=0)||!input(PROXIMITY)) // Udrzovani konstantni vzdalenosti od cihly |
if((vzdalenost!=0)||!input(PROXIMITY)||((delta_bearing>60)&&(delta_bearing<128))) // Udrzovani konstantni vzdalenosti od cihly |
{ |
if(cas>(CASMIN+20)) cas-=20; |
} |
128,11 → 131,12 |
bearing=i2c_read(0); |
i2c_stop(); |
|
if(touch==L_TOUCH) disp(0xC0); |
if(touch==R_TOUCH) disp(0x03); |
if(touch==L_TOUCH) visualisation|=0x2; |
if(touch==R_TOUCH) visualisation|=0x1; |
if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care; |
if((ridic==na_care)&&(touch==0)) break; |
if(ridic==na_care) touch=0; |
disp(visualisation); |
}; |
disp(0xC3); |
|