26,8 → 26,8 |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
for(n=1;n<=7;n++) |
{ |
while(get_timer1()<(odocounter+n)); // Popojed |
SetServoQ((CASAVR-CASMIN)); |
while(get_timer1()<(odocounter+n)); // Popojed |
SetServoQ((CASAVR-CASMIN)); |
}; |
|
i=0; |
35,7 → 35,7 |
disp(2); |
set_pwm1_duty(130); |
set_pwm2_duty(140); |
odocounter=get_timer1(); |
odocounter=get_timer1(); |
for(n=1;n<=10;n++) |
{ |
while(get_timer1()<(odocounter+n)); |
45,7 → 45,7 |
}; |
|
disp(3); |
odocounter=get_timer1(); |
odocounter=get_timer1(); |
for(n=1;n<=6;n++) |
{ |
while(get_timer1()<(odocounter+n)); |
55,7 → 55,7 |
}; |
|
disp(4); |
odocounter=get_timer1(); |
odocounter=get_timer1(); |
for(n=1;n<=6;n++) |
{ |
while(get_timer1()<(odocounter+n)); |
65,7 → 65,7 |
}; |
|
disp(5); |
odocounter=get_timer1(); |
odocounter=get_timer1(); |
for(n=1;n<=5;n++) |
{ |
while(get_timer1()<(odocounter+n)); |
83,53 → 83,12 |
for(n=1;n<=3;n++) |
{ |
while(get_timer1()<(odocounter+n)); |
SetServoQ((CASAVR-CASMIN)); |
SetServoQ((CASAVR-CASMIN)); |
}; |
|
disp(7); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+20)) // Popojed definovanou vzdalenos |
while(true) |
{ |
if(read_adc()<128) break; // Neprejeli jsme caru? |
}; |
set_pwm1_duty(0); // reverz (zabrzdi) |
set_pwm2_duty(0); |
output_high(MOT_L); |
output_high(MOT_R); |
delay_ms(300); |
brzda(); |
|
disp(8); |
SetServo(CASMIN); // max. doleva L |
set_pwm1_duty(0); // vzad |
set_pwm2_duty(0); |
output_low(MOT_L); |
output_high(MOT_R); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+10)) // Popojed definovanou vzdalenost |
{ |
if(read_adc()<128) break; // Neprejeli jsme caru? |
}; |
|
disp(9); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+3)); // Jeste trochu doprava, aby byla cara mezi krajnimi cidly |
set_pwm1_duty(0); // reverz (zabrzdi) |
set_pwm2_duty(255); |
output_low(MOT_L); |
output_low(MOT_R); |
delay_ms(150); |
brzda(); |
|
disp(10); |
SetServo((CASAVR-CASMIN)); // rovne S |
set_pwm1_duty(255); // Rozjezd na plny vykon vpred |
set_pwm2_duty(255); |
output_low(MOT_L); |
output_low(MOT_R); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+2)) // Uz jedeme? (prekonani sertvacne sily) |
{ |
set_adc_channel(LMAX); // Levy UV sensor |
delay_us(40); |
if(read_adc()<128) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo |
136,10 → 95,31 |
set_adc_channel(RMAX); // Pravy UV sensor |
delay_us(40); |
if(read_adc()<128) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo |
cas=CASAVR-CASMIN; // Cara je rovne |
}; |
} |
|
disp(11); |
if(cas==CASMIN) |
{ |
set_adc_channel(RMAX); // Pravy UV sensor |
delay_us(40); |
while(read_adc()>128); // Prejel caru? |
while(read_adc()<128); |
} |
else |
{ |
set_adc_channel(LMAX); // Levy UV sensor |
delay_us(40); |
while(read_adc()>128); // Prejel caru? |
while(read_adc()<128); |
} |
// Tady jsou s velkou pravdepodobnosti obe cidla za carou |
|
set_adc_channel(RMAX); |
SetServo(CASMAX-40); // stredne doprava |
while(read_adc()>128); |
|
|
cas=CASAVR-CASMIN; // Cara je rovne |
|
//stav=cihla; // Stav po objeti cihly, uz zadna cihla asi nebude |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
rr=60; // Nerozumna rychlost pro rozjeti |
//odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
//rr=60; // Nerozumna rychlost pro rozjeti |