| 0,0 → 1,167 |
|---|
| // **** Objeti cihly vpravo **** RRRR |
| int8 n,i; |
| int16 j; |
| |
| set_adc_channel(RMAX); |
| |
| SetServo(CASMIN); // max. doleva L |
| set_pwm1_duty(0); // vzad |
| set_pwm2_duty(0); |
| output_low(MOT_L); |
| output_high(MOT_R); |
| odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
| while(get_timer1()<(odocounter+8)); // Popojed definovanou vzdalenost |
| set_pwm1_duty(0); // reverz (zabrzdi) |
| set_pwm2_duty(255); |
| output_low(MOT_L); |
| output_low(MOT_R); |
| delay_ms(215); |
| brzda(); |
| |
| disp(1); |
| SetServo((CASAVR-CASMIN)); // rovne S |
| set_pwm1_duty(160); // vpred |
| set_pwm2_duty(160); |
| output_low(MOT_L); |
| output_low(MOT_R); |
| odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
| for(n=1;n<=7;n++) |
| { |
| while(get_timer1()<(odocounter+n)); // Popojed |
| SetServoQ((CASAVR-CASMIN)); |
| }; |
| |
| i=0; |
| |
| disp(2); |
| set_pwm1_duty(130); |
| set_pwm2_duty(140); |
| odocounter=get_timer1(); |
| for(n=1;n<=10;n++) |
| { |
| while(get_timer1()<(odocounter+n)); |
| SetServoQ((CASAVR-CASMIN)-i); |
| set_pwm1_duty(130-i); |
| i+=8; |
| }; |
| |
| disp(3); |
| odocounter=get_timer1(); |
| for(n=1;n<=7;n++) |
| { |
| while(get_timer1()<(odocounter+n)); |
| SetServoQ((CASAVR-CASMIN)-i); |
| set_pwm1_duty(130-i); |
| i-=8; |
| }; |
| |
| disp(4); |
| odocounter=get_timer1(); |
| for(n=1;n<=7;n++) |
| { |
| while(get_timer1()<(odocounter+n)); |
| SetServoQ((CASAVR-CASMIN)-i); |
| set_pwm1_duty(130-i); |
| i+=8; |
| }; |
| |
| disp(5); |
| odocounter=get_timer1(); |
| //!!!!for(n=1;n<=5;n++) |
| for(n=1;n<=4;n++) |
| { |
| while(get_timer1()<(odocounter+n)); |
| SetServoQ((CASAVR-CASMIN)-i); |
| set_pwm1_duty(130-i); |
| i-=16; |
| }; |
| |
| disp(6); |
| set_pwm1_duty(80); // vpred |
| set_pwm2_duty(80); |
| output_low(MOT_L); |
| output_low(MOT_R); |
| odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
| for(n=1;n<=3;n++) |
| { |
| while(get_timer1()<(odocounter+n)); |
| SetServoQ((CASAVR-CASMIN)); |
| }; |
| |
| disp(7); |
| while(true) |
| { |
| set_adc_channel(LMAX); // Levy UV sensor |
| delay_us(40); |
| if(read_adc()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {cas=CASMIN; break;}; }; // Prejeli jsme caru vlevo |
| set_adc_channel(RMAX); // Pravy UV sensor |
| delay_us(40); |
| if(read_adc()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {cas=CASMAX; break;}; }; // Prejeli jsme caru vpravo |
| } |
| |
| |
| if(cas==CASMIN) |
| { |
| set_adc_channel(RMAX); // Pravy UV sensor |
| for(j=0;j<10000;j++) |
| { |
| if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));}; // doleva |
| if (read_adc()<TRESHOLD) break; |
| } |
| } |
| else |
| { |
| set_adc_channel(LMAX); // Levy UV sensor |
| for(j=0;j<10000;j++) |
| { |
| if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));}; // doprava |
| if (read_adc()<TRESHOLD) break; |
| } |
| } |
| |
| 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 |
| |
| 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()>TRESHOLD); |
| 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()<TRESHOLD) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo |
| set_adc_channel(RMAX); // Pravy UV sensor |
| delay_us(40); |
| if(read_adc()<TRESHOLD) {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 |