10,9 → 10,14 |
enum okolo_cihly {pred_carou,na_care,po_care}; |
okolo_cihly ridic; // V jakem jsme stavu objizdeni cihly |
|
cihla: |
|
rr=20; |
|
disp(0x99); |
set_pwm1_duty(0); // zabrzdi levym kolem, prave vpred |
set_pwm2_duty(150); |
output_high(MOT_L); |
output_low(MOT_R); |
odocounter=get_timer1(); |
while(true) // Na zacatku se vyhni cihle, zatoc co muzes |
{ |
22,7 → 27,7 |
set_pwm2_duty(200); |
output_high(MOT_L); // leve kolo reverz |
output_low(MOT_R); // prave kolo vpred |
if(get_timer1()>(odocounter+5)) // konec zatacky? |
if(get_timer1()>(odocounter+8)) // konec zatacky? |
{ |
disp(0x66); |
cas=CASAVR-CASMIN; // rovne |
32,6 → 37,7 |
delay_ms(18); |
}; |
|
//------ Objeti cihly v konstantni vzdalenosti ------ |
touch=0; |
ridic=pred_carou; |
output_low(MOT_L); // vpred |
38,6 → 44,7 |
output_low(MOT_R); |
while(true) |
{ |
if(!input(IRRX)) goto cihla; |
|
if(!input(PROXIMITY)) // Udrzovani konstantni vzdalenosti od cihly |
{ |
78,9 → 85,9 |
}; |
disp(0xC3); |
|
set_pwm1_duty(0); |
set_pwm2_duty(170); |
output_low(MOT_L); |
set_pwm1_duty(20); |
set_pwm2_duty(200); |
output_high(MOT_L); |
output_low(MOT_R); |
delay_us(40); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
105,11 → 112,13 |
goto cara; |
}; |
} |
SetServoQ(CASMIN); // max. doleva L |
SetServoQ(CASMIN-5); // max. max. doleva L |
} |
|
cara: |
; |
|
output_low(MOT_L); // oba motory vpred |
output_low(MOT_R); |
/* |
set_pwm1_duty(0); // vypni motory |
set_pwm2_duty(0); |