0,0 → 1,100 |
// **** Objeti cihly vlevo **** LLLL |
|
int8 n; |
int8 r1,r2,rr; |
int1 flag; |
|
SetServo(CASMAX); // max. doleva, zatoc kolmo na caru |
set_pwm1_duty(0); |
set_pwm2_duty(0); |
output_high(MOT_L); // vzad |
output_low(MOT_R); |
disp(1); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+6)); // Popojed definovanou vzdalenost |
set_pwm1_duty(255); // reverz (zabrzdi) |
set_pwm2_duty(0); |
output_low(MOT_L); |
output_low(MOT_R); |
delay_ms(150); |
brzda(); |
|
set_pwm1_duty(80); |
set_pwm2_duty(80); |
set_adc_channel(LMAX); |
disp(2); |
cas=CASAVR-CASMIN; |
odocounter=get_timer1(); |
flag=1; |
rr=20; |
while(true) |
{ |
if(input(HREF)) |
{ |
if(!input(PROXIMITY)) |
{ |
if(cas>(CASMIN+10)) cas-=10; |
} |
else |
{ |
if(cas<(CASMAX-10)) cas+=10; |
}; |
|
SetServoQ(cas); |
|
// Elektronicky diferencial |
if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace |
if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;}; // pro rizeni rychlosti motoru |
if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92 |
|
if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN); // Neco jako nasobeni |
if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN); |
|
set_pwm1_duty(r1); |
set_pwm2_duty(r2); |
|
while(input(HREF)); |
}; |
if(get_timer1()>(odocounter+5)) // Prodleva, nez se zacne detekovat cara |
{ |
if(flag==1) {disp(3); flag=0;}; |
if(read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) break;};}; |
}; |
}; |
|
disp(4); |
while (read_adc()<THR_L); |
|
SetServo(CASMIN); // max. doleva L |
set_pwm1_duty(0); |
set_pwm2_duty(140); |
while (read_adc()>THR_L); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+3)); // Popojed definovanou vzdalenost |
set_pwm1_duty(0); // vypni motory |
set_pwm2_duty(0); |
/* |
set_pwm1_duty(0); // reverz (zabrzdi) |
set_pwm2_duty(0); |
output_low(MOT_L); |
output_high(MOT_R); |
*/ |
|
SetServo(CASAVR-CASMIN); // doprostred |
|
disp(5); |
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()<THR_L) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo |
set_adc_channel(RMAX); // Pravy UV sensor |
delay_us(40); |
if(read_adc()<THR_L) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo |
cas=CASAVR-CASMIN; // Cara je rovne |
}; |