| 7,8 → 7,8 |
|---|
| #USE FAST_IO (C) // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery |
| |
| // Rychlostni konstanty |
| #define RR_CIHLA 20 // Rozumna rychlost pro objizdeni cihly |
| #define RR_PRERUSENI 20 // Rozumna rychlost pro priblizeni se k preruseni |
| #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 |
| |
| // Adresy IIC periferii |
| 190,6 → 190,17 |
|---|
| output_low(SERVO); |
| } |
| |
| // Couvni po narazu na naraznik |
| inline void bum() |
| { |
| set_pwm1_duty(0); // couvni, rovne dozadu |
| set_pwm2_duty(0); |
| output_high(MOT_L); |
| output_high(MOT_R); |
| disp(0xA5); |
| SetServo(CASAVR-CASMIN); |
| } |
| |
| //---------------------------- INT -------------------------------- |
| #int_EXT |
| EXT_isr() // Preruseni od prekazky |
| 216,10 → 227,14 |
|---|
| return; |
| }; |
| |
| 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 |
| if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam |
| 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)|0x80; // Cihla ma nastaven nejvyssi bit |
| if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam |
| }; |
| //!!! Vyzkouset, ze nepreteka LOG |
| |
| //!!! if(stav==cihla) while(true); // Zastav na furt, konec drahy |
| 226,7 → 241,7 |
|---|
| // if(stav==cihla) return; // Po druhe nic neobjizdej |
| // Pozor na rozjezd |
| |
| if(stav==jizda) // Objed cihlu |
| if((stav==jizda)||(stav==cihla)) // Objed cihlu |
| { |
| #include ".\objizdka_L.c" |
| }; |
| 258,6 → 273,23 |
|---|
| output_low(MOT_L); // Nastav smer vpred |
| output_low(MOT_R); |
| |
| if(BUMPER) // Kdyz nekdo na zacatku drzi naraznik, vymaz log |
| { |
| int8 n; |
| |
| for(n=0;n<255;n++) write_eeprom(n,0xFF); |
| 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 |
| while(true) |
| { |
| disp(0x55); // Blikni pro potvrzeni |
| delay_ms(200); |
| disp(0xAA); |
| delay_ms(200); |
| } |
| } |
| |
| output_low(IRTX); // Zapni LED na detekci prekazky |
| |
| NightRider(1); // Aby se poznalo, ze byl RESET |
| 409,17 → 441,6 |
|---|
| log=0; // Zacatek logu v cerne skrince |
| last_log_odo=0; // Posledni zaznam odometrie do logu |
| |
| if(BUMPER) // Kdyz nekdo na zacatku drzi naraznik, vymaz log |
| { |
| 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 |
| disp(0x55); // Blikni pro potvrzeni |
| delay_ms(200); |
| disp(0xAA); |
| delay_ms(200); |
| } |
| |
| last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano? |
| bb=MAKE16(read_eeprom(1) & 0x7F,read_eeprom(0)); // Precti prvni zaznam |
| |
| 428,7 → 449,11 |
|---|
| { |
| int8 pom; |
| int8 n; |
| |
| int8 gap; |
| int16 ododo; |
| |
| gap=0; // Vynuluj pocitadlo preruseni |
| |
| next_snap: |
| |
| pom=0; |
| 464,16 → 489,28 |
|---|
| |
| if(pom==0) // Kamera nevidi caru, poznamenej to do logu |
| { |
| if(last_log_odo>get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu |
| if((cas>(CASMIN+30))&&(cas<(CASMAX-30))) // Nebyla minule cara moc u kraje? |
| if(last_log_odo<get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu |
| { |
| timer_pom=get_timer1(); // Timer se musi poznamenat atomicky |
| bb_l[log]=make8(timer_pom,0); // Zaznam |
| bb_h[log]=make8(timer_pom,1); |
| last_log_odo=timer_pom+8; // Dalsi mereni nejdrive po ujeti 24 cm |
| if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam |
| gap++; |
| if(gap>=3) // Trva preruseni alespon 3 snimky? |
| { |
| timer_pom=get_timer1(); // Timer se musi poznamenat atomicky |
| bb_l[log]=make8(timer_pom,0); // Zaznam |
| bb_h[log]=make8(timer_pom,1); |
| 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 |
| } |
| } |
| } |
| else gap=0; |
| |
| //ODODO |
| ododo=get_timer1(); |
| if((ododo>0x1C)&&(ododo<0x38)) rr=RR_PRERUSENI; |
| if((ododo>0x4F)&&(ododo<0x68)) 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 |
| 487,13 → 524,6 |
|---|
| // r1<<=1; // Rychlost je dvojnasobna |
| // r2<<=1; // Rozsah 2 az 184 |
| |
| /* Nerozumna rychlost po cihle |
| if ((stav==cihla)&&(get_timer1()>(odocounter+5))) // Snizime rychlost po ujeti |
| { |
| rr=rrold; |
| stav=pocihle; |
| }; |
| */ |
| if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle)) // Jizda |
| { |
| set_pwm1_duty(r1); |
| 549,12 → 579,7 |
|---|
| { |
| if(BUMPER) // Sakra, do neceho jsme narazili a nevideli jsme to! |
| { |
| set_pwm1_duty(0); // couvni, rovne dozadu |
| set_pwm2_duty(0); |
| output_high(MOT_L); |
| output_high(MOT_R); |
| disp(0x5A); |
| SetServo(CASAVR-CASMIN); |
| bum(); |
| SaveLog(log-1); // Zapis Black Boxu do EEPROM |
| output_low(MOT_L); // Vypni motory |
| output_low(MOT_R); |