| 0,0 → 1,247 |
|---|
| #include "D:\@KAKLIK\programy\PIC_C\robot\tank\main.h" |
| |
| //motory |
| #define FR output_high(PIN_B4);output_low(PIN_B5) // Vpred |
| #define FL output_high(PIN_B6);output_low(PIN_B7) |
| #define BR output_high(PIN_B5);output_low(PIN_B4) // Vzad |
| #define BL output_high(PIN_B7);output_low(PIN_B6) |
| #define STOPR output_low(PIN_B4);output_low(PIN_B5) |
| #define STOPL output_low(PIN_B6);output_low(PIN_B7) |
| |
| //cidla |
| #define RSENSOR 1 // Senzory na caru |
| #define LSENSOR 0 |
| //#define BUMPER SENSOR_2 // Senzor na cihlu |
| |
| //human interface |
| #DEFINE LED PIN_A2 |
| #DEFINE SOUND_HI PIN_B2 |
| #DEFINE SOUND_LO PIN_B3 |
| #include "sound.c" |
| |
| //program |
| #define MEZERA 38 // jak se muze jet dlouho bez cary |
| #define COUVEJ 700 // kolik se ma couvat po detekci diry |
| #define CIKCAK 40 // kolik se ma jet cik/cak |
| #define PRES_DIRU 350 // jak predpokladame velkou diru |
| #define TRESHOLD 50 |
| |
| #define L 0 // left |
| #define R 1 // right |
| #define S 2 // straight |
| |
| int movement; // smer minuleho pohybu |
| int line; // na ktere strane byla detekovana cara |
| int dira; // pocitadlo pro nalezeni preruseni cary |
| int n; // pomocna promena pro cyklus FOR |
| |
| void cik_cak() // Hledani cary |
| { |
| n=CIKCAK/2; // poprve hned zatoc opacne, nez se jelo pred detekci diry |
| switch(movement) // zmenime smer zataceni |
| { |
| case L: |
| FL;STOPR; // doprava |
| movement=R; // poznamenej kam jedem |
| line=L; // kdyz prejedem, tak bude cara vlevo |
| break; |
| case R: |
| FR;STOPL; // doleva |
| movement=L; // poznamenej kam jedem |
| line=R; // kdyz prejedem, tak bude cara vpravo |
| break; |
| } |
| |
| while(true) // jed cik-cak, dokud nenajdes caru |
| { |
| set_adc_channel(RSENSOR); |
| if (TRESHOLD > read_adc()) // je cara ? |
| { |
| STOPL; |
| STOPR; |
| break; |
| }; |
| if (CIKCAK < n++) // Jedeme uz dost dlouho cik? Pak jed cak. |
| { |
| n=0; |
| STOPL; |
| STOPR; |
| switch(movement) // zmenime smer zataceni |
| { |
| case L: |
| FL; // doprava |
| movement=R; // poznamenej kam jedem |
| line=L; // kdyz prejedem, tak bude cara vlevo |
| break; |
| case R: |
| FR; // doleva |
| movement=L; // poznamenej kam jedem |
| line=R; // kdyz prejedem, tak bude cara vpravo |
| break; |
| } |
| } |
| } |
| } |
| void diagnostika() |
| { |
| STOPL; //zastav vse |
| STOPR; |
| |
| FR; //pravy pas |
| Delay_ms(1000); |
| STOPR; |
| Delay_ms(1000); |
| BR; |
| Delay_ms(1000); |
| STOPR; |
| Delay_ms(1000); |
| |
| FL; //levy pas |
| Delay_ms(1000); |
| STOPL; |
| Delay_ms(1000); |
| BL; |
| Delay_ms(1000); |
| STOPL; |
| Delay_ms(1000); |
| |
| FL; //oba pasy |
| FR; |
| Delay_ms(1000); |
| STOPL; |
| STOPR; |
| Delay_ms(1000); |
| BL; |
| BR; |
| Delay_ms(1000); |
| STOPL; |
| STOPR; |
| Delay_ms(1000); |
| |
| } |
| |
| void main() |
| { |
| SoundBeep(640,200); //beep |
| Delay_ms(50); |
| SoundBeep(640,200); |
| |
| setup_adc_ports(RA0_RA1_RA3_ANALOG); |
| setup_adc(ADC_CLOCK_DIV_2); |
| Delay_ms(1000); // 1s |
| |
| movement=R; |
| line=S; |
| dira=0; |
| |
| //cik_cak(); // toc se, abys nasel caru |
| FL; FR; // vpred |
| |
| // Sledovani cary |
| while(true) |
| { |
| snimani: |
| set_adc_channel(RSENSOR); |
| Delay_us(10); |
| if (TRESHOLD > read_adc()) // Cara pod pravym senzorem |
| { |
| dira=0; // nuluj pocitadlo diry, protoze jsme videli caru |
| line=R; // zaznamenej, kdes videl caru |
| FL;FR; |
| goto snimani; |
| } |
| |
| set_adc_channel(LSENSOR); |
| Delay_us(10); |
| if (TRESHOLD > read_adc()) // Cara pod levym senzorem |
| { |
| dira=0; // nuluj pocitadlo diry, protoze jsme videli caru |
| line=L; // zaznamenej, kdes videl caru |
| FL;FR; |
| goto snimani; |
| } |
| |
| // oba senzory mimo caru |
| if (0==dira) // v prvnim cyklu po ztrate cary zacneme zahybat |
| // v ostatnich cyklech nedelame nic (pro urychleni snimani) |
| { |
| STOPL; |
| STOPR; |
| Delay_ms(100); |
| // kdyz nevidis caru, tak jed tam, kdes ji videl naposled |
| if(line==L) |
| { |
| // doleva |
| FR; |
| movement=L; // poznamenej kam jedes |
| } |
| if(line==R) |
| { |
| // doprava |
| FL; |
| movement=R; // poznamenej kam jedes |
| } |
| dira++; // zvets pocitadlo diry, aby to pristi cyklus tudy neslo |
| goto snimani; // co nejrychleji se vrat na snimani cary |
| } |
| |
| /*if (MEZERA < dira++) // nejedeme uz moc dlouho bez cary? |
| { |
| dira=0; |
| switch(movement) // kam se jelo ? |
| { |
| case L: |
| BR; // couvej doprava |
| Delay_ms(COUVEJ); |
| movement=R; |
| break; |
| case R: |
| BL; // couvej doleva |
| Delay_ms(COUVEJ); |
| movement=L; |
| break; |
| } |
| STOP(); // konec couvani |
| FL; FR; // rovne pres diru |
| Delay_ms(PRES_DIRU); |
| STOP(); |
| cik_cak(); // najdi caru |
| FR; FL; // vpred |
| } // dira*/ |
| |
| } // while(true) |
| |
| } |
| |
| /*void objizdka() |
| { |
| BL; BR; // zacouvej |
| Delay_ms(150); |
| STOP(); |
| |
| cik_cak(); // vyrovnej se na caru |
| cik_cak(); // radeji 3x, aby se vyrovnaly setrvacne sily |
| cik_cak(); |
| |
| BL; FR; Delay_ms(220); // vlevo |
| STOP(); |
| FL; FR; Delay_ms(650); // rovne |
| STOP(); |
| FL; BR; Delay_ms(220); // vpravo |
| STOP(); |
| FL; FR; Delay_ms(770); // rovne |
| STOP(); |
| FL; BR; Delay_ms(210); // vpravo |
| STOP(); |
| FL; FR; |
| Delay_ms(200); // kousek rovne |
| // rovne, dokud nenarazis na caru |
| while(input(RSENSOR)); // pravej senzor, protoze cihla je vpravo |
| STOP(); |
| BL; FR; // toc se doleva, dokud nenarazis na caru |
| while(input(LSENSOR)); // levej senzor, protoze cara je urcite vlevo |
| STOP(); |
| line=R; // caru predpokladame vpravo, kdybysme ji prejeli |
| dira=0; |
| FL; FR; // vpred |
| }*/ |