0,0 → 1,236 |
#include "main.h" |
|
int movement; // smer minuleho pohybu |
int line; // na ktere strane byla detekovana cara |
unsigned int8 dira; // pocitadlo pro nalezeni preruseni cary |
unsigned int8 speed; // rychlost zataceni |
unsigned int8 straight; // pocitadlo pro zjisteni rovneho useku |
|
// Konstanty pro dynamiku pohybu |
#define T_DIRA 16 // po jakem case zataceni se detekuje dira |
#define INC_SPEED 5 // prirustek rychlosti v jednom kroku |
#define RIGHT_ANGLE 1000 // 90 stupnu |
#define CIKCAK 20000 // 45 stupnu |
#define BW_PO_DIRE 200 // zpetny chod po dire |
#define FW_RYCHLE 200 // cara primo rovne |
#define FW_POMALU 100 // trochu mimo caru vnitrni pas |
#define FW_STREDNE 150 // trochu mimo caru vnejsi pas |
#define TURN_MIN 90 // minimalni rychlost pri zataceni |
#define TURN_MAX 150 // miximalni rychlost pri zataceni |
#define BRZDENI 40 // doba zpetneho chodu v ms, aby pas stal |
#define ROVINKA 8 // doba po kterou se musi jet rovne, aby se brzdilo |
|
//motory //Napred vypnout potom zapnout! |
#define FR output_low(PIN_B5); output_high(PIN_B4) // Vpred |
#define FL output_low(PIN_B7); output_high(PIN_B6) |
#define BR output_low(PIN_B4); output_high(PIN_B5) // Vzad |
#define BL output_low(PIN_B6); output_high(PIN_B7) |
#define STOPR output_low(PIN_B4);output_low(PIN_B5) // Zastav |
#define STOPL output_low(PIN_B6);output_low(PIN_B7) |
#define L 0b10 // left |
#define R 0b01 // right |
#define S 0b11 // straight |
|
//cidla |
#define RSENSOR 1 // Senzory na caru |
#define LSENSOR 0 |
#define BUMPER PIN_C4 // Senzor na cihlu |
|
#define DIAG_SERVO PIN_B0 // Propojka pro diagnosticky mod |
#define DIAG_SENSORS PIN_B1 // Propojka pro diagnosticky mod |
|
#DEFINE SOUND_HI PIN_B3 // komplementarni vystupy pro piezo pipak |
#DEFINE SOUND_LO PIN_B2 |
|
char AXstring[40]; // Buffer pro prenos telemetrie |
|
// makro pro PWM |
#define GO(motor, direction, power) if(get_timer0()<=power) \ |
{direction##motor;} \ |
else \ |
{stop##motor;} |
|
#int_TIMER1 // This function is called every time |
void TIMER1_isr() { // the RTCC (timer0) overflows (255->0). |
// For this program this is apx 76 times |
// per second. |
if (speed<TURN_MAX) speed+=INC_SPEED; |
if (dira<255) dira++; |
if (straight<255) straight++; |
} |
|
// Primitivni Pipani |
void beep(unsigned int16 period, unsigned int16 length) |
{ |
unsigned int16 nn; |
|
for(nn=length; nn>0; nn--) |
{ |
output_high(SOUND_HI);output_low(SOUND_LO); |
delay_us(period); |
output_high(SOUND_LO);output_low(SOUND_HI); |
delay_us(period); |
} |
} |
void diagnostika() |
{ |
unsigned int16 n; |
|
while (input(DIAG_SERVO)) // Propojka, ktera spousti diagnostiku |
{ |
for (n=500; n<800; n+=100) |
{ |
beep(n,n); //beep UP |
}; |
Delay_ms(1000); |
//zastav vse |
STOPL; STOPR; |
//pravy pas |
FR; Delay_ms(1000); STOPR; Delay_ms(1000); |
BR; Delay_ms(1000); STOPR; Delay_ms(1000); |
Beep(880,100); Delay_ms(1000); |
//levy pas |
FL; Delay_ms(1000); STOPL; Delay_ms(1000); |
BL; Delay_ms(1000); STOPL; Delay_ms(1000); |
Beep(880,100); Delay_ms(1000); |
//oba pasy |
FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000); |
BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000); |
}; |
|
while (input(DIAG_SENSORS)) |
{ |
int ls, rs; |
while(!input(BUMPER)){beep(1100,100); Delay_ms(50);} |
set_adc_channel(RSENSOR); |
Delay_us(20); |
rs=read_adc(); |
set_adc_channel(LSENSOR); |
Delay_us(20); |
ls=read_adc(); |
}; |
} |
void main() |
{ |
STOPL; STOPR; // prepne vystupy na ovladani motoru na output a zastavi |
|
setup_adc_ports(RA0_RA1_RA3_ANALOG); |
setup_adc(ADC_CLOCK_DIV_2); |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro PWM |
setup_timer_1(T1_INTERNAL|T1_DIV_BY_1); // Casovac pro regulaci |
setup_timer_2(T2_DISABLED,0,1); |
setup_ccp1(CCP_OFF); |
|
diagnostika(); |
|
Beep(1000,200); //double beep |
Delay_ms(50); |
Beep(1000,200); |
Delay_ms(1000); // 1s |
|
|
speed=TURN_MIN; // povoleni rizeni rychlosti zataceni pres preruseni |
enable_interrupts(INT_TIMER1); |
enable_interrupts(GLOBAL); |
|
// Cik-Cak ------------------------------------------------------------- |
line=S; |
movement=R; |
// cik_cak(); // toc se, abys nasel caru |
Delay_ms(500); |
Beep(1000,200); |
Delay_ms(500); |
|
dira=0; // inicializace globalnich promennych |
straight=255; |
|
while(true) // hlavni smycka (jizda podle cary) |
{ |
set_adc_channel(LSENSOR); // kdyz cara nebyla pod pravym cidlem, mozna bude pod levym |
Delay_us(10); |
if(tresholdL > read_adc()) |
{ |
movement = L; |
if (straight>ROVINKA) |
{GO(R, F, FW_STREDNE+40); GO(L, F, FW_POMALU+40)} // pridej |
else |
{GO(R, F, FW_STREDNE); GO(L, F, FW_POMALU)}; |
speed=TURN_MIN; |
dira=0; |
line=L; |
continue; |
} |
set_adc_channel(RSENSOR); // podivej se jestli neni cara pod pravym cidlem |
Delay_us(10); |
if(tresholdR > read_adc()) |
{ |
movement = R; |
if (straight>ROVINKA) |
{GO(L, F, FW_STREDNE+40); GO(R, F, FW_POMALU+40)} // pridej |
else |
{GO(L, F, FW_STREDNE); GO(R, F, FW_POMALU)}; |
speed=TURN_MIN; |
dira=0; |
line=R; |
continue; |
} |
if(line==S) |
movement = S; |
if (straight>ROVINKA) |
{FL; FR;} // pokud se jede dlouho rovne, tak pridej |
else |
{GO(R, F, FW_RYCHLE); GO(L, F, FW_RYCHLE)}; |
speed=TURN_MIN; // nastav minimalni rychlost pro zataceni |
dira=0; // protoze byla cara, tak nuluj pocitadlo diry |
continue; |
|
|
if (straight>ROVINKA) // pokud byla dlouha rovinka, tak zabrzdi |
{ |
BL; BR; |
Delay_ms(BRZDENI); |
STOPL; |
STOPR; |
dira=0; |
}; |
straight=0; // nuluj pocitadlo rovinky |
|
if (L==movement) // kdyz jsou obe cidla mimo caru, zatoc na caru |
{ |
GO(R, F, speed); |
STOPL; |
} |
else |
{ |
GO(L, F, speed); |
STOPR; |
} |
|
if (dira>T_DIRA) // pokud se moc dlouho zataci bez detekce cary, vrat se |
{ |
STOPL; |
STOPR; |
Beep(1000,200); //double beep |
Delay_ms(30); |
Beep(2000,200); |
Delay_ms(30); |
if (L==movement) // zpet, podle toho kam se jelo |
{ |
STOPL; |
BR; |
} |
else |
{ |
STOPR; |
BL; |
}; |
Delay_ms(BW_PO_DIRE); |
STOPL; |
STOPR; |
cik_cak(); // najdi caru |
dira=0; |
} |
|
} // while(true) |
} |
|