| 7,9 → 7,11 |
|---|
| #USE FAST_IO (C) // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery |
| |
| // Rychlostni konstanty |
| #define RR_CIHLA 24 // Rozumna rychlost pro objizdeni cihly |
| #define RR_PRERUSENI 18 // Rozumna rychlost pro priblizeni se k preruseni |
| #define BRZDNA_DRAHA 8 // Jak daleko pred problemem se zacne brzdit |
| #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 |
| |
| // Adresy IIC periferii |
| #define CAMERA_ADR 0xC0 |
| 44,8 → 46,8 |
|---|
| #define CASAVR ((CASMAX+CASMIN) / 2) |
| |
| #define EEMAX 255 // Konec EEPROM |
| #define MAXLOG 0x30 // Maximalni pocet zaznamu v logu |
| #if MAXLOG>(EEMAX/2-1) |
| #define MAXLOG 0x10 // Maximalni pocet zaznamu v logu |
| #if MAXLOG>(EEMAX/3) |
| #error Prekrocena velikost EEPROM |
| #endif |
| |
| 72,11 → 74,13 |
|---|
| int16 last_log; // Cislo posledniho zaznamu v logu v EEPROM |
| int8 bb_h[MAXLOG]; // Cerna skrinka MSB |
| 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 |
| |
| // Zobrazeni jednoho byte na modulu LEDbar |
| inline void disp(int8 x) |
| 122,12 → 126,15 |
|---|
| // Zaznam LOGu do EEPROM |
| void SaveLog(int8 log) |
| { |
| int8 n; |
| int8 n,i; |
| |
| for(n=0;n<=(log*2);n+=2) // Ulozeni Black Boxu do EEPROM |
| i=0; |
| for(n=0;n<=(log*3);n+=3) // Ulozeni Black Boxu do EEPROM |
| { |
| write_eeprom(n,bb_h[n/2]); |
| write_eeprom(n+1,bb_l[n/2]); |
| write_eeprom(n,bb_f[i]); |
| write_eeprom(n+1,bb_h[i]); |
| write_eeprom(n+2,bb_l[i]); |
| i++; |
| }; |
| write_eeprom(EEMAX,log); // Zapis poctu zaznamu na konec EEPROM |
| } |
| 164,7 → 171,7 |
|---|
| { |
| int8 n; |
| |
| for(n=0; n<14; n++) |
| for(n=0; n<10; n++) |
| { |
| output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo |
| delay_us(1000); |
| 232,7 → 239,8 |
|---|
| { |
| 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)|0x80; // Cihla ma nastaven nejvyssi bit |
| bb_h[log]=make8(timer_pom,1); |
| bb_f[log]=0xFF; // Cihla |
| if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam |
| }; |
| //!!! Vyzkouset, ze nepreteka LOG |
| 277,7 → 285,7 |
|---|
| { |
| int8 n; |
| |
| for(n=0;n<255;n++) write_eeprom(n,0xFF); |
| 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 |
| 431,7 → 439,7 |
|---|
| |
| set_adc_channel(ZELENA); // --- Kroutitko pro vykon motoru --- |
| delay_ms(1); |
| rr=read_adc()>>2; // 0-31 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni !!! |
| rr=read_adc()>>2; // 0-63 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni! |
| rrold=rr; |
| |
| cas=CASAVR-CASMIN; // Inicializace promenych, aby neslo servo za roh |
| 444,6 → 452,9 |
|---|
| last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano? |
| bb=MAKE16(read_eeprom(1) & 0x7F,read_eeprom(0)); // Precti prvni zaznam |
| |
| odo1=ODODO1-BRZDNA_DRAHA; |
| odo2=ODODO2-BRZDNA_DRAHA; |
| |
| // ........................... Hlavni smycka ................................ |
| while(true) |
| { |
| 457,7 → 468,7 |
|---|
| next_snap: |
| |
| pom=0; |
| disable_interrupts(GLOBAL); //----------------------- Critical |
| disable_interrupts(GLOBAL); //----------------------- Critical Section |
| while(input(HREF)); // Preskoc 1. radku |
| while(!input(HREF)); // Cekej nez se zacnou posilat pixely z 2. radky |
| set_timer0(0); // Vynuluj pocitadlo casu |
| 487,6 → 498,13 |
|---|
| delay_us(cas); |
| output_low(SERVO); |
| |
| // Elektronicky diferencial 1. cast |
| 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 |
| |
| enable_interrupts(GLOBAL); //----------------------- End Critical Section |
| |
| if(pom==0) // Kamera nevidi caru, poznamenej to do logu |
| { |
| if((cas>(CASMIN+30))&&(cas<(CASMAX-30))) // Nebyla minule cara moc u kraje? |
| 493,33 → 511,32 |
|---|
| if(last_log_odo<get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu |
| { |
| gap++; |
| if(gap>=3) // Trva preruseni alespon 3 snimky? |
| } |
| } |
| else |
| { |
| timer_pom=get_timer1(); // Timer se musi poznamenat atomicky |
| 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 |
| rr=rrold; // Preruseni skoncilo, vrat Rozumnou Rychlost |
| rr=rrold; // Preruseni cary skoncilo, znovu jed Rozumnou Rychlosti |
| } |
| gap=0; |
| } |
| } |
| else gap=0; |
| |
| //ODODO |
| ododo=get_timer1(); |
| if((ododo>0x1C)&&(ododo<0x38)) rr=RR_PRERUSENI; |
| if((ododo>0x4F)&&(ododo<0x68)) rr=RR_PRERUSENI; |
| if((ododo>odo1)&&(ododo<(odo1+8))) rr=RR_PRERUSENI; |
| if((ododo>odo2)&&(ododo<(odo2+8))) rr=RR_PRERUSENI; |
| |
| // 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 |
| |
| enable_interrupts(GLOBAL); //----------------------- Critical |
| |
| // 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); |
| 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 |
| |
| // r1<<=1; // Rychlost je dvojnasobna |
| // r2<<=1; // Rozsah 2 az 184 |
| 581,7 → 598,9 |
|---|
| { |
| bum(); |
| SaveLog(log-1); // Zapis Black Boxu do EEPROM |
| output_low(MOT_L); // Vypni motory |
| set_pwm1_duty(140); // pomalu vpred |
| set_pwm2_duty(140); |
| output_low(MOT_L); |
| output_low(MOT_R); |
| }; |
| set_adc_channel(LMAX); // Levy UV sensor |