4,12 → 4,12 |
int8 r1,r2,rr; |
int1 flag; |
|
SetServo(CASMIN-3); // max. doleva, zatoc kolmo na caru |
SetServo(CASMIN-5); // max. doleva, zatoc kolmo na caru |
set_pwm1_duty(0); |
set_pwm2_duty(140); |
output_low(MOT_L); // vpred |
output_low(MOT_R); |
disp(1); |
disp(0x81); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+6)); // Popojed definovanou vzdalenost |
|
16,7 → 16,7 |
set_pwm1_duty(80); |
set_pwm2_duty(80); |
set_adc_channel(LMAX); |
disp(2); |
disp(0x82); |
cas=CASAVR-CASMIN; |
odocounter=get_timer1(); |
flag=1; |
51,34 → 51,46 |
}; |
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;};}; |
if(flag==1) {disp(0x83); flag=0;}; |
if(read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) break;};}; // Neni to jen skvira nebo spina? |
}; |
}; |
|
disp(4); |
while(read_adc()<THR_L); // Prejed caru |
disp(0x84); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while((read_adc()<THR_L) || (get_timer1()<(odocounter+1))); // Popojed za caru |
SetServo(CASAVR); // Primy smer |
while((read_adc()<THR_L) || (get_timer1()<(odocounter+2))); // Popojed za caru |
|
SetServo(CASMIN); // max. doleva L |
set_pwm1_duty(0); |
set_pwm2_duty(140); |
while (read_adc()>THR_L); |
set_pwm1_duty(20); |
set_pwm2_duty(170); |
output_high(MOT_L); // levym kolem brzdi |
output_low(MOT_R); |
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
while(get_timer1()<(odocounter+3)); // Popojed definovanou vzdalenost |
while (true) // Znovu se musime dotknout cary |
{ |
if(read_adc()<THR_L) // Dotkli jsme se levym senzorem |
{ |
while(read_adc()<THR_L); // Prejedeme caru |
cas=CASAVR-CASMIN; // nastavime, ze cara je rovne |
break; |
}; |
set_adc_channel(RMAX); // Pravy UV sensor |
delay_us(40); |
if((get_timer1()>=(odocounter+2)) && (read_adc()<THR_L)) // Pravym senzorem nesmime caru prejet! |
{ |
cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo |
break; |
}; |
set_adc_channel(LMAX); // Levy UV sensor |
delay_us(40); |
} |
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); |
disp(0x85); |
set_pwm1_duty(255); // max. vpred |
set_pwm2_duty(255); |
output_low(MOT_L); |
92,5 → 104,5 |
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 |
// cas=CASAVR-CASMIN; // Cara je rovne |
}; |