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