| 9,6 → 9,7 |
|---|
| int8 touch; |
| enum okolo_cihly {pred_carou,na_care,po_care}; |
| okolo_cihly ridic; // V jakem jsme stavu objizdeni cihly |
| int8 vzdalenost; |
| |
| cihla: |
| |
| 27,10 → 28,9 |
|---|
| set_pwm2_duty(200); |
| output_high(MOT_L); // leve kolo reverz |
| output_low(MOT_R); // prave kolo vpred |
| if(get_timer1()>(odocounter+8)) // konec zatacky? |
| if(get_timer1()>(odocounter+5)) // konec zatacky? |
| { |
| disp(0x66); |
| cas=CASAVR-CASMIN; // rovne |
| break; |
| } |
| SetServoQ(cas); |
| 40,13 → 40,14 |
|---|
| //------ Objeti cihly v konstantni vzdalenosti ------ |
| touch=0; |
| ridic=pred_carou; |
| cas=CASAVR-CASMIN; // rovne |
| output_low(MOT_L); // vpred |
| output_low(MOT_R); |
| while(true) |
| { |
| if(!input(IRRX)) goto cihla; |
| // if(!input(IRRX)) goto cihla; |
| |
| if(!input(PROXIMITY)) // Udrzovani konstantni vzdalenosti od cihly |
| if((vzdalenost!=0)||!input(PROXIMITY)) // Udrzovani konstantni vzdalenosti od cihly |
| { |
| if(cas>(CASMIN+20)) cas-=20; |
| } |
| 68,6 → 69,12 |
|---|
| |
| SetServoQ(cas); |
| |
| i2c_start(); // Sonar Ping |
| i2c_write(0xE0); |
| i2c_write(0x0); |
| i2c_write(0x51); |
| i2c_stop(); |
| |
| for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva |
| { |
| set_adc_channel(LMAX); |
| 77,6 → 84,16 |
|---|
| delay_us(100); |
| if(read_adc()<THR) touch|=R_TOUCH; |
| }; |
| |
| i2c_start(); // Odraz ze sonaru |
| i2c_write(0xE0); |
| i2c_write(0x3); |
| i2c_stop(); |
| i2c_start(); |
| i2c_write(0xE1); |
| vzdalenost=i2c_read(0); |
| i2c_stop(); |
| |
| if(touch==L_TOUCH) disp(0xC0); |
| if(touch==R_TOUCH) disp(0x03); |
| if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care; |