| 9,9 → 9,10 |
|---|
| // Rychlostni konstanty |
| #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 0xFFF |
| #define ODODO2 0xFFF |
| #define BRZDNA_DRAHA 0x20 // Jak daleko pred problemem se zacne brzdit |
| #define ODODO_CIHLA 0xFFF |
| #define ODODO_TUNEL 0xFFF |
| #define ODODO_PRERUSENI 0xFFF |
| |
| // Adresy IIC periferii |
| #define COMPAS_ADR 0xC0 |
| 77,11 → 78,9 |
|---|
| int8 bb_l[MAXLOG]; // Cerna skrinka LSB |
| int8 bb_f[MAXLOG]; // Cerna skrinka priznak (typ zaznamu) |
| int8 log; // Pocitadlo pro cernou skrinku |
| int16 bb; // Posledni nactena udalost z Black Boxu |
| int16 timer_pom; // Pomocna promenna pro atomicke nacteni hodnoty timeru1 |
| int8 rr; // Promenna na ulozeni Rozumne rychlost |
| int8 rrold; |
| int16 odo1, odo2; // Problemy na trati |
| int16 odo_preruseni, odo_cihla, odo_tunel; // Problemy na trati |
| |
| // Zobrazeni jednoho byte na modulu LEDbar |
| inline void disp(int8 x) |
| 140,6 → 139,37 |
|---|
| write_eeprom(EEMAX,log); // Zapis poctu zaznamu na konec EEPROM |
| } |
| |
| // Zaznam do Logu do RAM |
| void LogLog(int8 flag, int16 gap) |
| { |
| int16 timer_pom; |
| |
| timer_pom=get_timer1(); // Timer se musi vycist atomicky |
| bb_l[log]=make8(timer_pom,0); // Zaznam |
| bb_h[log]=make8(timer_pom,1); |
| bb_f[log]=flag; // Typ zaznamu |
| if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam |
| last_log_odo=timer_pom+gap; // Dalsi mereni nejdrive po ujeti def. vzdalenosti |
| rr=rrold; // Problem skoncil, znovu jed Rozumnou Rychlosti |
| } |
| |
| void ReadBlackBox() |
| { |
| last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano? |
| { |
| int8 n,i; |
| |
| i=0; |
| for(n=0;n<=last_log;n++) |
| { |
| if(read_eeprom(i)==0) odo_tunel=MAKE16(read_eeprom(i+2),read_eeprom(i+1)); |
| if(read_eeprom(i)==0xFF) odo_cihla=MAKE16(read_eeprom(i+2),read_eeprom(i+1)); |
| if((read_eeprom(i)>0) && (read_eeprom(i)<0xFF)) odo_preruseni=MAKE16(read_eeprom(i+2),read_eeprom(i+1)); |
| } |
| } |
| } |
| |
| |
| // Brzdeni motorama stridou 1:1 |
| void brzda() |
| { |
| 209,6 → 239,8 |
|---|
| SetServo(CASAVR-CASMIN); |
| } |
| |
| #include ".\diag.c" |
| |
| //---------------------------- INT -------------------------------- |
| #int_EXT |
| EXT_isr() // Preruseni od prekazky |
| 232,11 → 264,11 |
|---|
| |
| i2c_start(); // Cteni kompasu |
| i2c_write(COMPAS_ADR); |
| i2c_write(0x1); |
| i2c_write(0x1); // 0-255 (odpovida 0-359) |
| i2c_stop(); |
| i2c_start(); |
| i2c_write(COMPAS_ADR+1); |
| bearing_offset=i2c_read(0); |
| bearing_offset=i2c_read(0); // Poznamenej hodnotu pred cihlou |
| i2c_stop(); |
| |
| delay_ms(9); |
| 250,11 → 282,7 |
|---|
| rr=rrold; // Po cihle se pojede opet Rozumnou Rychlosti |
| if(stav!=cihla) |
| { |
| 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_f[log]=0xFF; // Cihla |
| if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam |
| LogLog(0xFF,3); // Cihla |
| }; |
| |
| //!!! if(stav==cihla) while(true); // Zastav na furt, konec drahy |
| 295,50 → 323,15 |
|---|
| |
| disp(0); // Zhasni LEDbar |
| |
| if(BUMPER) // Kdyz nekdo na zacatku drzi naraznik, vymaz log |
| if(BUMPER) // Kdyz nekdo na zacatku drzi naraznik, vymaz log a spust diagnostiku |
| { |
| int8 n; |
| |
| for(n=0;n<255;n++) write_eeprom(n,0); |
| bb_l[0]=0; // Zapis na pozici 0 vzdalenost 0 |
| bb_h[0]=0; |
| SaveLog(0); // Zapis do EEPROM pocet zaznamu 0, tedy jeden zaznam |
| for(n=0;n<=4;n++) |
| { |
| disp(0x55); // Blikni pro potvrzeni |
| delay_ms(200); |
| disp(0xAA); |
| delay_ms(200); |
| }; |
| while(true) |
| { |
| if(!IRRX) |
| { |
| 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); |
| } |
| else |
| { |
| disp(0x81); // Diagnostika celniho IR senzoru na prekazku |
| delay_ms(200); |
| } |
| } |
| diag(); |
| } |
| |
| output_low(IRTX); // Zapni LED na detekci prekazky |
| |
| NightRider(1); // Aby se poznalo, ze byl RESET |
| // taky se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy |
| NightRider(1); // Zablikej, aby se poznalo, ze byl RESET |
| // Zaroven se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy |
| |
| //... Nastaveni sonaru ... |
| i2c_start(); |
| 352,28 → 345,6 |
|---|
| i2c_write(0x01); // male, pro eliminaci echa z minuleho mereni |
| i2c_stop(); |
| |
| // pro ladeni sonaru |
| /* |
| while(true) |
| { |
| i2c_start(); // Sonar Ping |
| i2c_write(0xE0); |
| i2c_write(0x0); |
| i2c_write(0x51); // 50 mereni v palcich, 51 mereni v cm, 52 v us |
| i2c_stop(); |
| delay_ms(100); |
| i2c_start(); // Odraz ze sonaru |
| i2c_write(0xE0); |
| i2c_write(0x3); |
| i2c_stop(); |
| i2c_start(); |
| i2c_write(0xE1); |
| cas=i2c_read(0); |
| i2c_stop(); |
| disp(cas); |
| } |
| */ |
| |
| //... Nastaveni kamery ... |
| i2c_start(); // Soft RESET kamery |
| i2c_write(CAMERA_ADR); // Adresa kamery |
| 486,11 → 457,11 |
|---|
| log=0; // Zacatek logu v cerne skrince |
| last_log_odo=0; // Posledni zaznam odometrie do logu |
| |
| last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano? |
| bb=MAKE16(read_eeprom(1) & 0x7F,read_eeprom(0)); // Precti prvni zaznam |
| // ReadBlackBox(); // Vycteni zaznamu z Black Boxu |
| |
| odo1=ODODO1-BRZDNA_DRAHA; |
| odo2=ODODO2-BRZDNA_DRAHA; |
| odo_cihla=ODODO_CIHLA-BRZDNA_DRAHA; |
| odo_tunel=ODODO_TUNEL-BRZDNA_DRAHA; |
| odo_preruseni=ODODO_PRERUSENI-BRZDNA_DRAHA; |
| |
| // ........................... Hlavni smycka ................................ |
| while(true) |
| 554,21 → 525,26 |
|---|
| { |
| if(gap>=2) // Trva preruseni cary alespon 2 snimky? |
| { |
| timer_pom=get_timer1(); // Timer se musi vycist atomicky |
| bb_l[log]=make8(timer_pom,0); // Zaznam |
| bb_h[log]=make8(timer_pom,1); |
| bb_f[log]=gap; |
| last_log_odo=timer_pom+8; // Dalsi mereni nejdrive po ujeti 24 cm |
| if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam |
| LogLog(gap,8); // Dalsi mereni nejdrive po ujeti 24 cm |
| rr=rrold; // Preruseni cary skoncilo, znovu jed Rozumnou Rychlosti |
| } |
| gap=0; |
| } |
| }; |
| |
| if(input(PROXIMITY) && ((stav==jizda)||(stav==cihla))) // Tunel |
| { |
| if(last_log_odo<get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu |
| { |
| LogLog(0,16); // Priznak tunelu; dalsi mereni nejdrive po ujeti 48 cm |
| rr=rrold; // Vjeli jsme do tunelu, znovu jed rychle |
| } |
| }; |
| |
| //ODODO |
| ododo=get_timer1(); |
| if((ododo>odo1)&&(ododo<(odo1+8))) rr=RR_PRERUSENI; |
| if((ododo>odo2)&&(ododo<(odo2+8))) rr=RR_PRERUSENI; |
| if((ododo>odo_preruseni)&&(ododo<(odo_preruseni+8))) rr=RR_PRERUSENI; |
| if((ododo>odo_cihla)&&(ododo<(odo_cihla+8))) rr=RR_PRERUSENI; |
| if((ododo>odo_tunel)&&(ododo<(odo_tunel+8))) rr=RR_PRERUSENI; |
| |
| // Elektronicky diferencial 2. cast |
| if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN); // Neco jako nasobeni |
| 576,7 → 552,7 |
|---|
| // rozsah 1 az 154 pro rr=63 |
| |
| // r1<<=1; // Rychlost je dvojnasobna |
| // r2<<=1; // Rozsah 2 az 184 |
| // r2<<=1; // Rozsah 2 az 184 pro rr=0 |
| |
| if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle)) // Jizda |
| { |
| 639,6 → 615,7 |
|---|
| set_pwm2_duty(160); |
| output_low(MOT_L); |
| output_low(MOT_R); |
| cas=CASAVR-CASMIN; |
| }; |
| set_adc_channel(LMAX); // Levy UV sensor |
| for(n=0;n<20;n++) if(input(HREF)) goto next_snap; |