| 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 |
| }; |