| 0,0 → 1,183 |
|---|
| // **** Objeti cihly vlevo **** LLLL |
| |
| #define L_TOUCH 1 // Cara vlevo |
| #define R_TOUCH 2 // Cata vpravo |
| #define B_TOUCH 3 // Both |
| |
| int8 n; |
| int8 r1,r2,rr; |
| int8 touch; |
| enum okolo_cihly {pred_carou,na_care,po_care}; |
| okolo_cihly ridic; // V jakem jsme stavu objizdeni cihly |
| int8 vzdalenost; |
| int8 visualisation; |
| |
| stav=cihla; // Dalsi prekazku uz nezaznamenavej (je to s velkou pravdepodobnosti cil) |
| odocounter=get_timer1(); |
| |
| cihla: |
| |
| rr=RR_CIHLA; //!!! Rozumna rychlost pro objizdeni cihly (bylo by lepsi rychlost zvysovat) a pri detekci pohybu zase snizit |
| disp(0x99); |
| set_pwm1_duty(0); // zabrzdi levym kolem, prave vpred |
| set_pwm2_duty(255); |
| output_high(MOT_L); |
| output_low(MOT_R); |
| while(true) // Na zacatku se vyhni cihle, zatoc co muzes |
| { |
| cas=CASMIN-5; // jeste vic nez hodne do leva |
| |
| if(BUMPER) // Narazili jsme do cihly, musime couvnout! |
| { |
| bum(); |
| SaveLog(log-1); // Zapis Black Boxu do EEPROM |
| delay_ms(TUHOS); //!!! Zatuhle prevodovky |
| brzda(); |
| goto cihla; // Znovu zacni cihlu objizdet |
| }; |
| |
| set_pwm1_duty(0); |
| set_pwm2_duty(200); // !!! mozna prilis maly vykon pro rozjezd pro zatuhlou prevodovku |
| output_high(MOT_L); // leve kolo reverz |
| output_low(MOT_R); // prave kolo vpred |
| if(get_timer1()>(odocounter+5)) // konec zatacky? |
| { |
| disp(0x66); |
| break; |
| } |
| SetServoQ(cas); |
| delay_ms(18); |
| }; |
| |
| //------ Objeti cihly v konstantni vzdalenosti ------ |
| touch=0; // Indikator detekce cary pri objizdeni |
| ridic=pred_carou; |
| cas=CASAVR-CASMIN; // rovne |
| output_low(MOT_L); // vpred |
| output_low(MOT_R); |
| visualisation=0; |
| while(true) |
| { |
| if(BUMPER) // Narazili jsme do cihly, musime couvnout! |
| { |
| bum(); |
| SaveLog(log-1); // Zapis Black Boxu do EEPROM |
| delay_ms(TUHOS); //!!! Zatuhle prevodovky |
| set_pwm1_duty(160); // vpred |
| set_pwm2_duty(160); |
| output_low(MOT_L); |
| output_low(MOT_R); |
| cas=CASMIN; |
| }; |
| |
| delta_bearing=bearing-bearing_offset; |
| visualisation=(delta_bearing & 0xF0) | (visualisation & 0x0F); |
| if(IRRX) // hrozi celni srazka s cihlou v prubehu objizdeni |
| { |
| cas=CASMIN; |
| } |
| else |
| { |
| if((vzdalenost!=0)||!input(PROXIMITY)||((delta_bearing>60)&&(delta_bearing<128))) // Udrzovani konstantni vzdalenosti od cihly |
| { |
| if(cas>(CASMIN+30)) cas-=30; |
| } |
| else |
| { |
| if(cas<(CASMAX-30)) cas+=30; |
| }; |
| }; |
| // 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); |
| |
| //!!! pro zatuhle prevodovky |
| // r1<<=1; // Rychlost je dvojnasobna |
| // r2<<=1; // Rozsah 2 az 184 pro rr=0 |
| |
| set_pwm1_duty(r1); // Nastav rychlost motoru |
| set_pwm2_duty(r2); |
| |
| SetServoQ(cas); |
| |
| i2c_start(); // Sonar Ping |
| i2c_write(SONAR_ADR); |
| i2c_write(0x0); |
| i2c_write(0x52); // mereni v us |
| i2c_stop(); |
| |
| for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva |
| { |
| set_adc_channel(LMAX); |
| delay_us(100); |
| if(read_adc()<THR) touch|=L_TOUCH; |
| set_adc_channel(RMAX); |
| delay_us(100); |
| if(read_adc()<THR) touch|=R_TOUCH; |
| }; |
| |
| i2c_start(); // Odraz ze sonaru |
| i2c_write(SONAR_ADR); |
| i2c_write(0x3); |
| i2c_stop(); |
| i2c_start(); |
| i2c_write(SONAR_ADR+1); |
| vzdalenost=i2c_read(0); |
| i2c_stop(); |
| |
| i2c_start(); // Cteni kompasu |
| i2c_write(COMPAS_ADR); |
| i2c_write(0x1); // uhel 0-255 |
| i2c_stop(); |
| i2c_start(); |
| i2c_write(COMPAS_ADR+1); |
| bearing=i2c_read(0); |
| i2c_stop(); |
| |
| if(touch==L_TOUCH) visualisation|=0x2; |
| if(touch==R_TOUCH) visualisation|=0x1; |
| if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care; |
| if((ridic==na_care)&&(touch==0)) break; |
| if(ridic==na_care) touch=0; |
| disp(visualisation); |
| }; |
| disp(0xC3); |
| |
| set_pwm1_duty(0); //!!! pred zatuhlejma prevodovkama tam bylo 20 a 200 |
| set_pwm2_duty(240); |
| output_high(MOT_L); |
| output_low(MOT_R); |
| delay_us(40); |
| odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
| while (true) // Znovu se musime dotknout cary |
| { |
| for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva |
| { |
| set_adc_channel(LMAX); // Levy UV sensor |
| delay_us(100); |
| if(read_adc()<THR) // Dotkli jsme se levym senzorem |
| { |
| disp(0xE0); |
| cas=CASAVR-CASMIN; // nastavime, ze cara je rovne |
| goto cara; |
| }; |
| set_adc_channel(RMAX); // Pravy UV sensor |
| delay_us(100); |
| if((get_timer1()>=(odocounter+2)) && (read_adc()<THR)) // Pravym senzorem nesmime caru prejet! |
| { |
| disp(0x07); |
| cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo |
| goto cara; |
| }; |
| } |
| SetServoQ(CASMIN-5); // max. max. doleva L |
| } |
| |
| cara: |
| |
| output_low(MOT_L); // oba motory vpred |
| output_low(MOT_R); |