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 |
}*/ |