1,5 → 1,6 |
// **** Objeti cihly vpravo **** RRRR |
int8 n,i; |
int16 j; |
|
set_adc_channel(RMAX); |
|
97,29 → 98,69 |
if(read_adc()<128) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo |
} |
|
|
if(cas==CASMIN) |
{ |
set_adc_channel(RMAX); // Pravy UV sensor |
delay_us(40); |
while(read_adc()>128); // Prejel caru? |
while(read_adc()<128); |
for(j=0;j<10000;j++) |
{ |
if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));}; // doleva |
if (read_adc()<128) break; |
} |
} |
else |
{ |
set_adc_channel(LMAX); // Levy UV sensor |
delay_us(40); |
while(read_adc()>128); // Prejel caru? |
while(read_adc()<128); |
set_adc_channel(LMAX); // Pravy UV sensor |
for(j=0;j<10000;j++) |
{ |
if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));}; // doprava |
if (read_adc()<128) break; |
} |
} |
// Tady jsou s velkou pravdepodobnosti obe cidla za carou |
|
set_adc_channel(RMAX); |
SetServo(CASMAX-40); // stredne doprava |
while(read_adc()>128); |
set_adc_channel(RMAX); // Pravy UV sensor |
|
set_pwm1_duty(0); // zabrzdi |
set_pwm2_duty(0); |
output_high(MOT_L); |
output_high(MOT_R); |
SetServo(CASAVR-CASMIN); // doprostred |
delay_ms(100); |
brzda(); |
// Tady jsou s velkou pravdepodobnosti obe cidla za carou a jsme kolmo k care |
|
cas=CASAVR-CASMIN; // Cara je rovne |
SetServo(CASMIN); // max. doleva L |
set_pwm1_duty(0); // vzad |
set_pwm2_duty(0); |
output_low(MOT_L); |
output_high(MOT_R); |
while (read_adc()>128); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost |
set_pwm1_duty(0); // reverz (zabrzdi) |
set_pwm2_duty(255); |
output_low(MOT_L); |
output_low(MOT_R); |
SetServo(CASAVR-CASMIN); // doprostred |
delay_ms(100); |
brzda(); |
|
set_pwm1_duty(255); // max. 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)) // Ujed kousek |
{ |
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 |
}; |
|
//stav=cihla; // Stav po objeti cihly, uz zadna cihla asi nebude |
//odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
//rr=60; // Nerozumna rychlost pro rozjeti |