0,0 → 1,342 |
#include ".\main.h" |
|
#define KOLMO1 225 // predni kolecko sroubem dopredu |
#define KOLMO2 30 // predni kolecko je hlavou sroubu dozadu |
#define STRED 128 // sredni poloha zataceciho kolecka |
#define BEAR1 10//10 // 3 stupne zataceni |
#define BEAR2 25//27 |
#define BEAR3 45 |
#define R 100 // Rozumna rychlost |
#define R17 255 // X nasobek rozumne rychlosti |
#define DOZNIVANI 10 |
#define L1 1 // cara vlevo |
#define L2 2 // cara vlevo |
#define L3 3 // cara vlevo |
#define S 0 // casa mezi sensory |
#define R1 -1 // cara vpravo |
#define R2 -2 // cara vpravo |
#define R3 -3 // cara vpravo |
|
// servo |
#define SERVO PIN_B5 |
|
// kroutitka |
#define CERVENA 5 // AN5 |
#define CERNA 2 // AN2 |
#define ZELENA 4 // AN4 |
#define MODRA 6 // AN6 |
|
// IR |
#define IRTX PIN_B2 |
#define CIHLA PIN_A3 |
|
//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 |
signed int8 line = S; // 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 turn; // rychlost toceni |
unsigned int8 rovinka; // pocitadlo na zjisteni rovinky |
|
short int preteceni; // flag preteceni timeru1 |
|
signed int16 Lmotor; // promene, ktere urcuji velikost vykonu na levem |
signed int16 Rmotor; // a pravem motoru |
|
// makro pro PWM pro motory |
#define GO(motor, direction, power) if(get_timer0()<=power) \ |
{direction##motor;} else {stop##motor;} |
|
//////////////////////////////////////////////////////////////////////////////// |
#int_TIMER1 |
TIMER1_isr() |
{ |
preteceni = true; |
} |
//////////////////////////////////////////////////////////////////////////////// |
#int_TIMER2 |
TIMER2_isr() // ovladani serva |
{ |
unsigned int8 n; |
|
output_high(SERVO); |
delay_us(1000); |
for(n=uhel; n>0; n--) Delay_us(2); |
output_low(SERVO); |
} |
|
//////////////////////////////////////////////////////////////////////////////// |
short int IRcheck() // potvrdi detekci cihly |
{ |
output_high(IRTX); // vypne vysilac IR |
delay_ms(100); |
|
output_low(STROBE); |
sensors = spi_read(0); // cteni senzoru |
sensors=~sensors; |
output_high(STROBE); |
|
if(true==bit_test(sensors,7)) // otestuje, jestli je stale detekovan IR signal |
{ |
output_low(IRTX); // zapne vysilac IR |
delay_ms(100); |
|
output_low(STROBE); |
sensors = spi_read(0); // cteni senzoru |
sensors=~sensors; |
output_high(STROBE); |
|
if(false==bit_test(sensors,7)) // otestuje, jestli je detekovana cihla |
{ |
output_high(IRTX); // vypne vysilac IR |
delay_ms(100); |
|
output_low(STROBE); |
sensors = spi_read(0); // cteni senzoru |
sensors=~sensors; |
output_high(STROBE); |
|
output_low(IRTX); // zapne vysilac IR |
if(bit_test(sensors,7)) return 1; // vrat 1, kdyz je stale cihla |
} |
}; |
output_low(IRTX); // zapne vysilac IR |
return 0; // vrat 0, kdyz je detekovano ruseni |
} |
//////////////////////////////////////////////////////////////////////////////// |
void objizdka() |
{ |
int8 shure=0; |
unsigned int16 n; |
|
BR;BL; |
Delay_ms(400); |
STOPR;STOPL; |
|
// toceni na miste dokud nezmizi cihla |
//------------------------------------ |
uhel=KOLMO1; // nastav zataceci kolecko kolmo na osu robota |
Delay_ms(100); |
BL;FR; |
Delay_ms(200); // minimalni toceni, kdyby se zastavilo sikmo k cihle |
|
While(bit_test(sensors,7)) // toc, dokud neni cihla z primeho senzoru |
{ |
sensors = spi_read(0); // cteni senzoru |
sensors=~sensors; |
Delay_ms(4); // cekani na SLAVE nez pripravi data od cidel |
} |
STOPL; STOPR; |
|
for (n=0;n<1000;n++) // vystred se na hranu cihly |
{ |
if(!input(CIHLA)) |
{ |
GO(L,B,180);GO(R,F,160); // zapni motory PWM podle promenych Lmotor a Rmotor |
} else |
{ |
GO(L,F,180);GO(R,B,160); // zapni motory PWM podle promenych Lmotor a Rmotor |
}; |
delay_ms(1); |
} |
STOPR;STOPL; |
|
uhel=STRED; // dopredu |
delay_ms(100); |
FR; FL; |
delay_ms(500); |
BL;BR; |
delay_ms(200); |
STOPL;STOPR; |
|
uhel=STRED+BEAR3; // doprava |
delay_ms(100); |
FL; |
delay_ms(400); |
uhel=STRED+BEAR2; // min doprava |
FL;FR; |
delay_ms(100); |
uhel=STRED+BEAR1; // jeste min doprava |
FL;FR; |
delay_ms(200); |
While((sensors & 0b01111111)!=0) //dokud neni cara |
{ |
sensors = spi_read(0); // cteni senzoru |
sensors=~sensors; |
Delay_ms(4); // cekani na SLAVE nez pripravi data od cidel |
} |
BL; BR; |
delay_ms(400); |
|
uhel=STRED-BEAR3; // doleva |
} |
|
//////////////////////////////////////////////////////////////////////////////// |
void main() |
{ |
|
unsigned int8 n; |
unsigned int8 i,v; |
unsigned int8 last_sensors; |
|
setup_adc_ports(sAN5|sAN2|sAN4|sAN6|VSS_VDD); // AD pro kroutitka |
setup_adc(ADC_CLOCK_INTERNAL); |
setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_16); |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
setup_timer_1(T1_DISABLED|T1_DIV_BY_8); |
setup_timer_2(T2_DIV_BY_16,140,16); |
setup_oscillator(OSC_8MHZ|OSC_INTRC); |
|
STOPR; STOPL; // zastav motory |
Lmotor=0;Rmotor=0; |
|
uhel = STRED; // nastav zadni kolecko na stred |
rovinka = 0; |
|
enable_interrupts(INT_TIMER2); |
enable_interrupts(GLOBAL); |
|
output_low(IRTX); // zapni IR vysilac |
|
delay_ms(1500); // musime pockat na diagnostiku slave CPU |
|
//nastaveni rychlosti |
set_adc_channel(CERVENA); |
Delay_ms(1); |
speed=R+(read_adc()>>2); // rychlost rovne +63; kroutitko dava 0-63 |
set_adc_channel(MODRA); |
Delay_ms(1); |
turn=speed-32+(read_adc()>>2); // rychlost toceni +-32; kroutitko dava 0-63 |
while(true) |
{ |
|
GO(L,F,Lmotor);GO(R,F,Rmotor); // zapni motory PWM podle promenych Lmotor a Rmotor |
|
delay_us(2000); // cekani na SLAVE, nez pripravi data od cidel |
|
output_low(STROBE); // vypni zobrazovani na posuvnem registru |
sensors = spi_read(0); // cteni senzoru |
sensors=~sensors; |
output_high(STROBE); // zobraz data na posuvnem registru |
|
i=0; // havarijni kod |
for (n=0; n<=6; n++) |
{ |
if(bit_test(sensors,n)) i++; |
} |
if (i>3) // zastavi, kdyz je cerno pod vice nez tremi cidly |
{ |
BL; BR; |
delay_ms(300); |
STOPR; STOPL; |
While(true); |
}; |
|
if(bit_test(sensors,7)) // detekce cihly |
{ |
objizdka(); // objede cihlu |
} |
|
|
if(bit_test(sensors,3)) //...|...// |
{ |
uhel=STRED; |
if (rovinka<50) |
{ |
Lmotor=R17; |
Rmotor=R17; |
} |
else |
{ |
Lmotor=speed; |
Rmotor=speed; |
}; |
line=S; |
if (rovinka < 255) rovinka++; |
continue; |
} |
|
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=turn; |
line=L3; |
continue; |
} |
|
if(bit_test(sensors,6)) //......|// |
{ |
uhel=STRED + BEAR3; |
Rmotor=0; |
Lmotor=turn; |
line=R3; |
continue; |
} |
|
if(bit_test(sensors,1)) //.|.....// |
{ |
uhel=STRED - BEAR2; |
Lmotor=speed-50; |
Rmotor=speed; |
line=L2; |
continue; |
} |
|
if(bit_test(sensors,5)) //.....|.// |
{ |
uhel=STRED + BEAR2; |
Rmotor=speed-50; |
Lmotor=speed; |
line=R2; |
continue; |
} |
|
if (bit_test(sensors,2)) //..|....// |
{ |
uhel=STRED - BEAR1; |
Lmotor=speed; |
Rmotor=speed; |
line=L1; |
if (rovinka<255) rovinka++; |
continue; |
} |
|
if (bit_test(sensors,4)) //....|..// |
{ |
uhel=STRED + BEAR1; |
Rmotor=speed; |
Lmotor=speed; |
line=R1; |
if (rovinka<255) rovinka++; |
continue; |
} |
|
|
if ((L3==line) || (R3==line)) // Brzdeni pri vyjeti z trate |
{ |
if (rovinka>50) |
{ |
BL; BR; |
Delay_ms(100); |
if (rovinka>250) delay_ms(50); |
}; |
rovinka=0; |
}; |
} |
} |