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