1,5 → 1,7 |
// **** Objeti cihly vpravo **** RRRR |
|
set_adc_channel(RMAX); |
|
SetServo(CASMIN); // max. doleva L |
set_pwm1_duty(0); // vzad |
set_pwm2_duty(10); |
6,7 → 8,7 |
output_low(MOT_L); |
output_high(MOT_R); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+10)); // Popojed definovanou vzdalenost |
while(get_timer1()<(odocounter+8)); // Popojed definovanou vzdalenost |
set_pwm1_duty(0); // reverz (zabrzdi) |
set_pwm2_duty(255); |
output_low(MOT_L); |
20,75 → 22,57 |
output_low(MOT_L); |
output_low(MOT_R); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+10)); // Popojed definovanou vzdalenost |
set_pwm1_duty(0); // reverz (zabrzdi) |
set_pwm2_duty(0); |
output_high(MOT_L); |
output_high(MOT_R); |
delay_ms(250); |
brzda(); |
while(get_timer1()<(odocounter+5)); // Popojed definovanou vzdalenost |
|
SetServo(CASMIN); // max. doleva L |
set_pwm1_duty(0); // vpred |
set_pwm2_duty(220); |
output_low(MOT_L); |
output_low(MOT_R); |
SetServoQ((CASAVR-CASMIN)-20); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+7)); // Popojed definovanou vzdalenost |
set_pwm1_duty(0); // reverz (zabrzdi) |
set_pwm2_duty(0); |
output_low(MOT_L); |
output_high(MOT_R); |
delay_ms(250); |
brzda(); |
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost |
set_pwm1_duty(140); |
set_pwm1_duty(120); |
|
SetServo((CASAVR-CASMIN)); // rovne S |
set_pwm1_duty(160); // vpred |
set_pwm2_duty(160); |
output_low(MOT_L); |
output_low(MOT_R); |
SetServoQ((CASAVR-CASMIN)-30); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+10)); // Popojed definovanou vzdalenost |
set_pwm1_duty(0); // reverz (zabrzdi) |
set_pwm2_duty(0); |
output_high(MOT_L); |
output_high(MOT_R); |
delay_ms(250); |
brzda(); |
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost |
set_pwm1_duty(100); |
|
SetServo(CASMIN); // max. doleva L |
set_pwm1_duty(0); // vpred |
set_pwm2_duty(220); |
output_low(MOT_L); |
output_low(MOT_R); |
SetServoQ((CASAVR-CASMIN)-60); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+7)); // Popojed definovanou vzdalenost |
set_pwm1_duty(0); // reverz (zabrzdi) |
set_pwm2_duty(0); |
output_low(MOT_L); |
output_high(MOT_R); |
delay_ms(250); |
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost |
set_pwm1_duty(80); |
|
set_adc_channel(RMAX); // A/D prevodnik na cteni praveho UV cidla |
brzda(); |
SetServoQ((CASAVR-CASMIN)-40); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost |
set_pwm1_duty(80); |
|
SetServo((CASAVR-CASMIN)); // rovne S |
set_pwm1_duty(160); // vpred |
set_pwm2_duty(160); |
output_low(MOT_L); |
output_low(MOT_R); |
SetServoQ((CASAVR-CASMIN)-70); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+9)) // Popojed definovanou vzdalenost |
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost |
set_pwm1_duty(80); |
|
SetServoQ((CASAVR-CASMIN)-60); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost |
set_pwm1_duty(100); |
|
SetServoQ((CASAVR-CASMIN)-30); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost |
set_pwm1_duty(120); |
|
SetServoQ((CASAVR-CASMIN)); // Rovne |
set_pwm1_duty(80); // vpred |
set_pwm2_duty(80); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+10)) // Popojed definovanou vzdalenos |
{ |
if(read_adc()<128) break; // Neprejeli jsem caru? |
}; |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+3)); // Popojed definovanou vzdalenost |
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(250); |
delay_ms(300); |
brzda(); |
|
SetServo(CASMIN); // max. doleva L |
99,10 → 83,10 |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+10)) // Popojed definovanou vzdalenost |
{ |
if(read_adc()<128) break; // Neprejeli jsem caru? |
if(read_adc()<128) break; // Neprejeli jsme caru? |
}; |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+3)); // Popojed definovanou vzdalenost |
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); |
111,11 → 95,22 |
brzda(); |
|
SetServo((CASAVR-CASMIN)); // rovne S |
set_pwm1_duty(255); // vpred |
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 |
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 |
}; |
|
cas=CASAVR-CASMIN; // Cara je rovne |
|
//!!!!!!!!!!!!!!!!! stav=cihla; // Stav po objeti cihly, uz zadna cihla nebude |
stav=cihla; // Stav po objeti cihly, uz zadna cihla asi nebude |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
rr=60; // Nerozumna rychlost pro rozjeti |