Rev 2 | Go to most recent revision | Blame | Last modification | View Log | Download
#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
}*/