0,0 → 1,275 |
#include ".\main.h" |
|
#use rtos (timer=2, minor_cycle=2ms) |
|
#define STRED 128 // sredni poloha zadniho kolecka |
#define BEAR1 12 // 3 stupne zataceni |
#define BEAR2 34 |
#define BEAR3 70 |
#define SPEEDMAX 120 // maximalni rychlost |
|
#define L 1 |
#define S 2 |
#define R 0 |
|
// servo |
#define SERVO PIN_A2 |
|
// IR |
#define IRTX PIN_B2 |
|
//motory |
#define FR output_low(PIN_A7); output_high(PIN_A6) // Vpred |
#define FL output_low(PIN_A1); output_high(PIN_A0) |
#define BR output_low(PIN_A6); output_high(PIN_A7) // Vzad |
#define BL output_low(PIN_A0); output_high(PIN_A1) |
#define STOPR output_low(PIN_A6);output_low(PIN_A7) |
#define STOPL output_low(PIN_A0);output_low(PIN_A1) |
|
//HID |
#define LED1 PIN_B1 //oranzova |
#define LED2 PIN_B2 //zluta |
|
#define STROBE PIN_B0 |
//#define SW1 PIN_A2 // Motory On/off |
|
unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru |
unsigned int8 line; // na ktere strane byla detekovana cara |
//unsigned int8 dira; // pocita dobu po kterou je ztracena cara |
unsigned int8 uhel; // urcuje aktualni uhel zataceni |
//unsigned int8 speed; // maximalni povolena rychlost |
unsigned int8 rovinka; // pocitadlo na zjisteni rovinky |
|
signed int16 Lmotor; // promene, ktere urcuji velikost vykonu na levem |
signed int16 Rmotor; // a pravem motoru |
|
// makro pro PWM |
#define GO(motor, direction, power) if(get_timer0()<=power) \ |
{direction##motor;} else {stop##motor;} |
|
//////////////////////////////////////////////////////////////////////////////// |
//////////////////////////////////////////////////////////////////////////////// |
/* |
void diagnostika() |
{ |
if(input(SW1))STOPR;STOPL;While(TRUE); |
// if(LSENSOR==true) output_high(LED2); else output_low(LED2); |
// if(RSENSOR==true) output_high(LED1); else output_low(LED1); |
} |
*/ |
//////////////////////////////////////////////////////////////////////////////// |
#task (rate=18ms,max=2ms) |
void zatoc() // ridi servo ktere otaci kolem |
{ |
unsigned int8 n; |
|
output_high(SERVO); |
delay_us(1100); |
for(n=uhel; n>0; n--); |
output_low(SERVO); |
} |
|
//////////////////////////////////////////////////////////////////////////////// |
void diag() // ridi servo ktere otaci kolem |
{ |
unsigned int8 n; |
|
output_high(SERVO); |
delay_us(1100); |
for(n=uhel; n>0; n--); |
output_low(SERVO); |
} |
|
//////////////////////////////////////////////////////////////////////////////// |
short int IRcheck() // potvrdi detekci cihly |
{ |
output_high(IRTX); // vypne vysilac IR |
|
output_low(STROBE); |
sensors = spi_read(0); // cteni senzoru |
output_high(STROBE); |
|
if(true==bit_test(sensors,7)) // otestuje, jestli je stale detekovan IR signal |
{ |
output_low(IRTX); // zapne vysilac IR |
|
output_low(STROBE); |
sensors = spi_read(0); // cteni senzoru |
output_high(STROBE); |
|
if(false==bit_test(sensors,7)) // otestuje, jestli je detekovana cihla |
{ |
output_high(IRTX); // vypne vysilac IR |
|
output_low(STROBE); |
sensors = spi_read(0); // cteni senzoru |
output_high(STROBE); |
|
if(bit_test(sensors,7)) return 1; // |
|
else return 0; // vrat 0 kdyz je ruseni |
} |
else return 0; // vrat 0 kdyz to nebyla cihla |
} |
else return 0; // vrat 0 kdyz je detekovano ruseni |
} |
//////////////////////////////////////////////////////////////////////////////// |
void ojizdka() |
{ |
|
} |
//////////////////////////////////////////////////////////////////////////////// |
/* |
#task (rate=4ms, max=20us) |
void rychlost() |
{ |
if(speed<255) speed++; |
} |
*/ |
//////////////////////////////////////////////////////////////////////////////// |
#task (rate=2ms, max=1ms) |
void rizeni() |
{ |
unsigned int16 n; |
unsigned int8 i; |
|
GO(L,F,Lmotor);GO(R,F,Rmotor); // zapni motory |
|
delay_us(500); |
|
output_low(STROBE); |
sensors = spi_read(0); // cteni senzoru |
sensors=~sensors; |
output_high(STROBE); |
|
i=0; // havarijni kod |
for (n=0; n<=6; n++) |
{ |
if(bit_test(sensors,n)) i++; |
} |
if (i>3) rtos_terminate(); // zastavi, kdyz je cerno pod vice nez tremi cidly |
|
|
if(bit_test(sensors,3)) //...|...// |
{ |
uhel=STRED; |
Lmotor=SPEEDMAX; |
Rmotor=SPEEDMAX; |
line=S; |
if (rovinka<255) rovinka++; |
return; |
} |
|
if(bit_test(sensors,0)) //|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu |
{ |
uhel=STRED-BEAR3; |
Lmotor=0; |
Rmotor=SPEEDMAX; |
line=L; |
return; |
} |
|
if(bit_test(sensors,6)) //......|// |
{ |
uhel=STRED+BEAR3; |
Rmotor=0; |
Lmotor=SPEEDMAX; |
line=R; |
return; |
|
} |
|
if(bit_test(sensors,1)) //.|.....// |
{ |
uhel=STRED-BEAR2; |
Lmotor=SPEEDMAX-50; |
Rmotor=SPEEDMAX; |
line=S; |
return; |
} |
|
if(bit_test(sensors,5)) //.....|.// |
{ |
uhel=STRED+BEAR2; |
Rmotor=SPEEDMAX-50; |
Lmotor=SPEEDMAX; |
line=S; |
return; |
} |
|
if (bit_test(sensors,2)) //..|....// |
{ |
uhel=STRED-BEAR1; |
Lmotor=SPEEDMAX; |
Rmotor=SPEEDMAX; |
line=S; |
if (rovinka<255) rovinka++; |
return; |
} |
|
if (bit_test(sensors,4)) //....|..// |
{ |
uhel=STRED+BEAR1; |
Rmotor=SPEEDMAX; |
Lmotor=SPEEDMAX; |
line=S; |
if (rovinka<255) rovinka++; |
return; |
} |
|
if ((L==line) || (R==line)) // Brzdeni pri vyjeti z trate |
{ |
if (rovinka>250) |
{ |
BL; BR; |
for(n=1; n<(60); n++) {rtos_yield(); delay_us(500);}; |
}; |
rovinka=0; |
} |
|
if(bit_test(sensors,7)) // zjisti jestli neni cihla |
{ |
rtos_terminate(); |
} |
} |
|
|
|
//////////////////////////////////////////////////////////////////////////////// |
void main() |
{ |
unsigned int16 i; |
unsigned int8 last; |
|
setup_adc_ports(NO_ANALOGS); |
setup_adc(ADC_CLOCK_INTERNAL); |
setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_4); |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
setup_timer_1(T1_DISABLED); |
setup_oscillator(OSC_8MHZ|OSC_INTRC); |
|
STOPR; STOPL; // zastav motory |
Lmotor=0;Rmotor=0; |
|
uhel=STRED; // nastav zadni kolecko na stred |
rovinka=0; |
|
output_low(IRTX); // zapni IR vysilac |
|
for(i=0;i<100;i++) // pockej, nez se zadni kolecko vystredi |
{ |
diag(); |
delay_ms(16); |
} |
// diagnostika(); |
|
while(true) |
{ |
rtos_run(); |
STOPR; |
STOPL; |
If(IRcheck()) objizdka(); |
|
while(true); |
} |
} |