| 24,6 → 24,7 |
|---|
| #define DATA PIN_B2 // K modulu LEDbar data |
| #define CP PIN_B1 // K modulu LEDbar hodiny |
| //#define ODO PIN_A4 // Ze snimace z odometrie z praveho kola / nove je to na RC0 na TIMER1 |
| // Jeden impuls je 31,25mm |
| #define IRRX PIN_B0 // Vstup INT, generuje preruseni pri prekazce |
| #define IRTX PIN_B3 // Modulovani vysilaci IR LED na detekci prekazky |
| #define PROXIMITY PIN_C7 // Cidlo kratkeho dosahu na cihlu |
| 42,11 → 43,13 |
|---|
| #bit INT0IF = INTCON.1 |
| #bit RBIF = INTCON.0 |
| |
| enum stavy {start,rozjezd,jizda,cihla,cil}; |
| enum stavy {start,rozjezd,jizda,cihla,pocihle,cil}; |
| stavy stav; // Kde jsme na trati |
| int8 cas; // Cas hrany bila/cerna v radce |
| int8 stred; // Vystredeni kolecka |
| int16 odocounter; // Zaznamenani aktualniho stavu pocitadla odometrie |
| int8 rr; // Promenna na ulozeni Rozumne rychlost |
| int8 rrold; |
| |
| // Zobrazeni jednoho byte na modulu LEDbar |
| void disp(int8 x) |
| 136,12 → 139,24 |
|---|
| } |
| } |
| |
| inline void SetServoQ(int8 angle) |
| { |
| output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo |
| delay_us(1000); |
| delay_us(stred); |
| delay_us(stred); |
| delay_us(stred); |
| delay_us(angle); |
| delay_us(angle); |
| output_low(SERVO); |
| } |
| |
| #int_EXT |
| EXT_isr() // Preruseni od prekazky |
| { |
| set_pwm1_duty(0); // reverz (zabrzdi) |
| set_pwm1_duty(0); // zabrzdi |
| set_pwm2_duty(0); |
| output_high(MOT_L); // vzad |
| output_high(MOT_L); |
| output_high(MOT_R); |
| // Ujistime se, ze prijaty signal je z naseho IR vysilace |
| output_high(IRTX); // Vypni LED na detekci prekazky |
| 160,7 → 175,7 |
|---|
| output_low(MOT_R); |
| return; |
| }; |
| |
| /* |
| SetServo((CASAVR-CASMIN)); // rovne |
| |
| set_pwm1_duty(140); // vpred |
| 167,10 → 182,8 |
|---|
| set_pwm2_duty(140); |
| output_low(MOT_L); |
| output_low(MOT_R); |
| |
| odocounter=get_timer1(); // Poznamenej aktualni stav odometrie |
| //!!! while(true) {disp(MAKE8(get_timer1(),0)); delay_ms(10);}; |
| |
| while(true) |
| { |
| if(!input(PROXIMITY)) break; // Je cihla blizko? |
| 180,10 → 193,11 |
|---|
| set_pwm2_duty(0); |
| output_high(MOT_L); |
| output_high(MOT_R); |
| */ |
| delay_ms(150); |
| brzda(); |
| |
| if (stav==cihla) while(true); // Zastav na furt, konec drahy |
| if(stav==pocihle) stav=jizda;//!!!!while(true); // Zastav na furt, konec drahy |
| |
| if(stav==jizda) // Objed cihlu |
| { |
| 196,7 → 210,6 |
|---|
| void main() |
| { |
| int8 offset; // Promena pro ulozeni ovsetu |
| int8 rr; // Promenna na ulozeni Rozumne rychlost |
| int8 r1; // Rychlost motoru 1 |
| int8 r2; // Rychlost motoru 2 |
| int16 trasa; // Pocitadlo ujete vzdalenosti |
| 326,7 → 339,8 |
|---|
| 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 !!! |
| |
| rrold=rr; |
| |
| cas=CASAVR-CASMIN; // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska |
| stav=start; |
| trasa=0; |
| 341,6 → 355,7 |
|---|
| next_snap: |
| |
| pom=0; |
| disable_interrupts(GLOBAL); //----------------------- Critical |
| while(input(HREF)); // Preskoc 1. radku |
| while(!input(HREF)); // Cekej nez se zacnou posilat pixely z 2. radky |
| set_timer0(0); // Vynuluj pocitadlo casu |
| 375,6 → 390,8 |
|---|
| 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 |
| |
| 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); |
| |
| 381,8 → 398,14 |
|---|
| // r1<<=1; // Rychlost je dvojnasobna |
| // r2<<=1; // Rozsah 2 az 184 |
| |
| if ((stav==jizda)||(stav==cihla)) // Jizda |
| if ((stav==cihla)&&(get_timer1()>(odocounter+5))) |
| { |
| rr=rrold; |
| stav=pocihle; |
| }; |
| |
| if ((stav==jizda)||(stav==cihla)||(stav==pocihle)) // Jizda |
| { |
| set_pwm1_duty(r1); |
| set_pwm2_duty(r2); |
| trasa++; |
| 391,7 → 414,7 |
|---|
| { |
| set_pwm1_duty(0); // Zastaveni |
| set_pwm2_duty(0); |
| } |
| }; |
| |
| //!!! trasu zrusit a predelat to na odometrii |
| if((stav==jizda)&&(trasa>50)) // musi to alespon 1s jet |
| 419,19 → 442,19 |
|---|
| stav=jizda; |
| }; |
| } |
| |
| |
| pom=0x80; // Zobrazeni pozice cary na displayi |
| for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1; |
| disp(pom); |
| |
| while(true) |
| while(true) // Ve zbytku casu snimku cti krajni UV senzory |
| { |
| set_adc_channel(LMAX); // Levy UV sensor |
| for(n=0;n<20;n++) if(input(HREF)) goto next_snap; |
| for(n=0;n<20;n++) if(input(HREF)) goto next_snap; |
| if(read_adc()<128) cas=CASMIN; |
| set_adc_channel(RMAX); // Pravy UV sensor |
| for(n=0;n<20;n++) if(input(HREF)) goto next_snap; |
| for(n=0;n<20;n++) if(input(HREF)) goto next_snap; |
| if(read_adc()<128) cas=CASMAX; |
| }; |
| }; |
| } |
| } |