| 10,11 → 10,12 |
|---|
| #define RR_CIHLA 30 // Rozumna rychlost pro objizdeni cihly |
| #define RR_PRERUSENI 20 // Rozumna rychlost pro priblizeni se k preruseni |
| #define BRZDNA_DRAHA 0x20 // Jak daleko pred problemem se zacne brzdit |
| #define ODODO1 0x37 |
| #define ODODO2 0x5E |
| #define ODODO1 0x37 |
| #define ODODO2 0x5E |
| |
| // Adresy IIC periferii |
| #define CAMERA_ADR 0xC0 |
| #define COMPAS_ADR 0xC0 |
| #define CAMERA_ADR 0xDC |
| #define SONAR_ADR 0xE0 |
| |
| // A/D vstupy |
| 212,6 → 213,8 |
|---|
| #int_EXT |
| EXT_isr() // Preruseni od prekazky |
| { |
| int8 bearing, bearing_offset, delta_bearing; |
| |
| set_pwm1_duty(0); // zabrzdi levym kolem, prave vypni |
| set_pwm2_duty(0); |
| output_high(MOT_L); |
| 226,7 → 229,17 |
|---|
| return; |
| }; |
| output_low(IRTX); // Zapni LED na detekci prekazky |
| delay_ms(10); |
| |
| i2c_start(); // Cteni kompasu |
| i2c_write(COMPAS_ADR); |
| i2c_write(0x1); |
| i2c_stop(); |
| i2c_start(); |
| i2c_write(COMPAS_ADR+1); |
| bearing_offset=i2c_read(0); |
| i2c_stop(); |
| |
| delay_ms(9); |
| if (!IRRX) // stale nas signal? |
| { |
| output_low(MOT_L); // neni odraz -> vpred |
| 239,11 → 252,10 |
|---|
| { |
| timer_pom=get_timer1(); // Obsah Timeru1 se musi nacist atomicky |
| bb_l[log]=make8(timer_pom,0); // Zaznam do logu |
| bb_h[log]=make8(timer_pom,1); |
| bb_h[log]=make8(timer_pom,1); |
| bb_f[log]=0xFF; // Cihla |
| if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam |
| }; |
| //!!! Vyzkouset, ze nepreteka LOG |
| |
| //!!! if(stav==cihla) while(true); // Zastav na furt, konec drahy |
| // if(stav==cihla) return; // Po druhe nic neobjizdej |
| 281,6 → 293,26 |
|---|
| output_low(MOT_L); // Nastav smer vpred |
| output_low(MOT_R); |
| |
| disp(0); // Zhasni LEDbar |
| |
| /*!!!! |
| while(true) |
| { |
| int8 ble; |
| |
| i2c_start(); // Cteni kompasu |
| i2c_write(COMPAS_ADR); |
| i2c_write(0x1); |
| i2c_stop(); |
| i2c_start(); |
| i2c_write(COMPAS_ADR+1); |
| ble=i2c_read(0); |
| i2c_stop(); |
| disp(ble); |
| delay_ms(200); |
| } |
| //*/ |
| |
| if(BUMPER) // Kdyz nekdo na zacatku drzi naraznik, vymaz log |
| { |
| int8 n; |
| 291,10 → 323,18 |
|---|
| SaveLog(0); // Zapis do EEPROM pocet zaznamu 0, tedy jeden zaznam |
| while(true) |
| { |
| disp(0x55); // Blikni pro potvrzeni |
| delay_ms(200); |
| disp(0xAA); |
| delay_ms(200); |
| if(!IRRX) |
| { |
| disp(0x55); // Blikni pro potvrzeni |
| delay_ms(200); |
| disp(0xAA); |
| delay_ms(200); |
| } |
| else |
| { |
| disp(0x81); // Diagnostika celniho IR senzoru na prekazku |
| delay_ms(200); |
| } |
| } |
| } |
| |
| 339,7 → 379,7 |
|---|
| |
| //... Nastaveni kamery ... |
| i2c_start(); // Soft RESET kamery |
| i2c_write(CAMERA_ADR); // Pro single slave musi mit vsechny zapisy adresu C0h |
| i2c_write(CAMERA_ADR); // Adresa kamery |
| i2c_write(0x12); // Adresa registru COMH |
| i2c_write(0x80 | 0x24); // Zapis ridiciho slova |
| i2c_stop(); |
| 352,13 → 392,13 |
|---|
| |
| /* |
| i2c_start(); // Contrast (nema podstatny vliv na obraz) |
| i2c_write(0xC0); |
| i2c_write(CAMERA_ADR); |
| i2c_write(0x05); |
| i2c_write(0xA0); // 48h |
| i2c_stop(); |
| |
| i2c_start(); // Band Filter (pokud by byl problem se zarivkama 50Hz) |
| i2c_write(0xC0); |
| i2c_write(CAMERA_ADR); |
| i2c_write(0x2D); |
| i2c_write(0x04 | 0x03); |
| i2c_stop(); |
| 454,7 → 494,7 |
|---|
| |
| odo1=ODODO1-BRZDNA_DRAHA; |
| odo2=ODODO2-BRZDNA_DRAHA; |
| |
| |
| // ........................... Hlavni smycka ................................ |
| while(true) |
| { |
| 536,7 → 576,7 |
|---|
| // Elektronicky diferencial 2. cast |
| 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); // rozsah 1 az 92 pro rr=0 |
| // rozsah 1 az 154 pro rr=63 |
| // rozsah 1 az 154 pro rr=63 |
| |
| // r1<<=1; // Rychlost je dvojnasobna |
| // r2<<=1; // Rozsah 2 az 184 |
| 600,7 → 640,7 |
|---|
| SaveLog(log-1); // Zapis Black Boxu do EEPROM |
| set_pwm1_duty(140); // pomalu vpred |
| set_pwm2_duty(140); |
| output_low(MOT_L); |
| output_low(MOT_L); |
| output_low(MOT_R); |
| }; |
| set_adc_channel(LMAX); // Levy UV sensor |