/Designs/ROBOTS/3Orbis/SW/cidla/cidla.BAK |
---|
0,0 → 1,158 |
#include ".\cidla.h" |
//#include <stdlib.h> |
#use rs232(baud=9600,parity=N,xmit=PIN_B3,bits=8,restart_wdt) |
#define IRRX PIN_B0 |
#define TRESHOLD_MAX 70 // rozhodovaci uroven pro cidla cerna/bila |
#define TRESHOLD_MIN 50 |
#define CIHLA 10 // doba, po kterou musi byt detekovana cihla |
unsigned int8 radius; // co cidla vidi |
unsigned int8 last_radius; // rozsah |
unsigned int8 last_cidla; // co cidla videla minule |
unsigned int8 shure; // citac doby, po kterou musi byt detekovana cihla |
//tuning |
/*#define PULZACE 3 // urcuje rychlost pulzovani pomoci PWM |
//Vystup PWM je na PIN_B3 |
//////////////////////////////////////////////////////////////////////////////// |
void pulzovani() // postupne rozsvecuje a zhasina podsvetleni |
{ |
unsigned int8 i,n; |
for(n=0;n<=3;n++) |
{ |
for(i=0;i<255;i++) {set_pwm1_duty(i); Delay_ms(PULZACE);} // rozsvecovani |
for(i=255;i>0;i--) {set_pwm1_duty(i); Delay_ms(PULZACE);} // zhasinani |
} |
} |
*/ |
//////////////////////////////////////////////////////////////////////////////// |
void main() |
{ |
int8 cidla; |
unsigned int8 a; |
unsigned int8 n; |
setup_adc_ports(sAN0|sAN1|sAN2|sAN3|sAN4|sAN5|sAN6|VSS_VDD); |
setup_adc(ADC_CLOCK_INTERNAL); |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
setup_timer_1(T1_DISABLED); |
setup_timer_2(T2_DISABLED,0,1); |
setup_comparator(NC_NC_NC_NC); |
setup_vref(FALSE); |
Delay_ms(500); |
setup_spi(SPI_SLAVE|SPI_H_TO_L|SPI_SS_DISABLED); |
// diagnostika |
printf("\n\r"); |
Delay_ms(100); |
printf("***\n\r"); |
Delay_ms(100); |
for (n=0; n<=6; n++) |
{ |
set_adc_channel(n); |
Delay_ms(100); |
a=read_adc(); |
printf("sensor %u - %u\n\r",n,a); |
} |
shure=0; |
while(true) |
{ |
set_adc_channel(0); |
cidla=0; |
Delay_us(10); |
a=read_adc(); |
set_adc_channel(1); |
if(a<TRESHOLD_MAX) //hystereze cidel |
{ |
if(a>TRESHOLD_MIN) |
{ |
cidla |= (last_cidla & 0b00000001); |
} |
else cidla |= 0b00000001; |
} |
a=read_adc(); |
set_adc_channel(2); |
if(a<TRESHOLD_MAX) |
{ |
if(a>TRESHOLD_MIN) |
{ |
cidla |= (last_cidla & 0b00000010); |
} |
else cidla |= 0b00000010; |
} |
a=read_adc(); |
set_adc_channel(3); |
if(a<TRESHOLD_MAX) |
{ |
if(a>TRESHOLD_MIN) |
{ |
cidla |= (last_cidla & 0b00000100); |
} |
else cidla |= 0b00000100; |
} |
a=read_adc(); |
set_adc_channel(4); |
if(a<TRESHOLD_MAX) |
{ |
if(a>TRESHOLD_MIN) |
{ |
cidla |= (last_cidla & 0b00001000); |
} |
else cidla |= 0b00001000; |
} |
a=read_adc(); |
set_adc_channel(5); |
if(a<TRESHOLD_MAX) |
{ |
if(a>TRESHOLD_MIN) |
{ |
cidla |= (last_cidla & 0b00010000); |
} |
else cidla |= 0b00010000; |
} |
a=read_adc(); |
set_adc_channel(6); |
if(a<TRESHOLD_MAX) |
{ |
if(a>TRESHOLD_MIN) |
{ |
cidla |= (last_cidla & 0b00100000); |
} |
else cidla |= 0b00100000; |
} |
a=read_adc(); |
if(a<TRESHOLD_MAX) |
{ |
if(a>TRESHOLD_MIN) |
{ |
cidla |=(last_cidla & 0b01000000); |
} |
else cidla |= 0b01000000; |
} |
last_cidla=cidla; |
if (!input(IRRX)) {if (shure<255) shure++;} else {shure=0;}; |
if (shure>CIHLA) cidla|=0b10000000; |
cidla=~cidla; |
spi_write(cidla); |
} |
} |
/Designs/ROBOTS/3Orbis/SW/main.BAK |
---|
0,0 → 1,288 |
#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//25 |
#define BEAR3 45//45 |
#define R 100 // Rozumna rychlost |
#define R17 200 // X nasobek rozumne rychlosti |
#define PRED_CIHLOU 100 // rychlost pri dalkove detekci cihly |
//#define L1 1 // cara vlevo |
#define L2 2 // cara vlevo |
#define L3 3 // cara vlevo |
#define S 0 // cara 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 4 // AN4 |
//#define CERNA 5 // AN5 |
//#define ZELENA 6 // AN6 |
#define MODRA 2 // AN2 |
// 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 |
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_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); |
} |
//////////////////////////////////////////////////////////////////////////////// |
int8 IRcheck() // potvrdi detekci cihly |
{ |
output_high(IRTX); // vypne vysilac IR |
delay_ms(10); |
output_low(STROBE); |
sensors = spi_read(0); // cteni senzoru |
sensors=~sensors; |
output_high(STROBE); |
if(bit_test(sensors,7)) // otestuje, jestli je stale detekovan IR signal |
{ |
output_low(IRTX); // zapne vysilac IR |
delay_ms(10); |
output_low(STROBE); |
sensors = spi_read(0); // cteni senzoru |
sensors=~sensors; |
output_high(STROBE); |
if(!bit_test(sensors,7)) // otestuje, jestli je detekovana cihla |
{ |
output_high(IRTX); // vypne vysilac IR |
delay_ms(10); |
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 |
} |
//////////////////////////////////////////////////////////////////////////////// |
#include ".\objizdka_centrovani.c" |
//////////////////////////////////////////////////////////////////////////////// |
void main() |
{ |
unsigned int8 n; |
unsigned int8 i,j; |
unsigned int8 last_sensors; |
unsigned int8 RozumnaRychlost; |
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(2000); // musime pockat na diagnostiku slave CPU |
//nastaveni rychlosti |
set_adc_channel(CERVENA); |
Delay_ms(1); |
RozumnaRychlost=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 |
speed=R17; |
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 |
last_sensors=sensors; |
output_low(STROBE); // vypni zobrazovani na posuvnem registru |
sensors = spi_read(0); // cteni senzoru |
sensors=~sensors; // neguj prijata data |
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>4) // zastavi, kdyz je cerno pod vice nez tremi cidly |
{ |
BL; BR; |
delay_ms(300); |
STOPR; STOPL; |
While(true); |
}; |
/* |
if (!input(CIHLA)) // dalkova detekce cihly |
{ |
speed=PRED_CIHLOU; |
} |
else |
{ |
speed=RozumnaRychlost; |
} |
*/ |
if (bit_test(sensors,7)) // detekce cihly |
{ |
BR;BL; |
Delay_ms(400); |
STOPR;STOPL; |
// if (1==IRcheck()) // kontrola, jestli nebylo rusene cidlo |
{ |
Delay_ms(100); |
cikcak(); |
delay_ms(100); |
objizdka(); // objede cihlu |
} |
} |
if (speed > RozumnaRychlost) speed--; // postupne zpomaleni na Roz. Rychl. |
if(bit_test(sensors,3)) //...|...// |
{ |
uhel=STRED; |
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-70; |
Rmotor=speed; |
line=L2; |
continue; |
} |
if(bit_test(sensors,5)) //.....|.// |
{ |
uhel=STRED + BEAR2; |
Rmotor=speed-70; |
Lmotor=speed; |
line=R2; |
continue; |
} |
if (bit_test(sensors,2)) //..|....// |
{ |
uhel=STRED - BEAR1; |
Lmotor=speed-20; |
Rmotor=speed; |
line=L2; |
if (rovinka<255) rovinka++; |
continue; |
} |
if (bit_test(sensors,4)) //....|..// |
{ |
uhel=STRED + BEAR1; |
Rmotor=speed-20; |
Lmotor=speed; |
line=L2; |
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 || speed > 170) delay_ms(50); |
}; |
rovinka=0; |
speed=R17; |
}; |
} |
} |
/Designs/ROBOTS/3Orbis/SW/objizdka_centrovani.BAK |
---|
0,0 → 1,131 |
#define DOLEVA 0 |
#define DOPRAVA 1 |
void cikcak() |
{ |
unsigned int8 i=0; |
uhel=KOLMO1; // aby se dalo tocit na miste |
Delay_ms(100); |
if (line==L2) line=L3; // poznamenej, kde byla cara pred brzdenim |
if (line==S) line=L3; |
if (line==R2) line=R3; |
Delay_ms(3); // prodleva na cteni senzoru pred prenosem |
output_low(STROBE); // zapni zobrazovani na posuvnem registru |
sensors = spi_read(0); // cteni senzoru |
sensors=~sensors; |
output_high(STROBE); // vypni zobrazovani na posuvnem registru |
while(true) |
{ |
Delay_ms(3); |
if (line==L3) |
{ |
GO(L,B,160);GO(R,F,160); |
}; |
if (line==R3) |
{ |
GO(R,B,160);GO(L,F,160); |
}; |
if (line==S) {STOPL;STOPR; i++;} else i=0; |
if (i>=100) break; // pokud je dostatecne dlouho cara vprostred, vypadni |
output_low(STROBE); // zapni zobrazovani na posuvnem registru |
sensors = spi_read(0); // cteni senzoru |
sensors=~sensors; |
output_high(STROBE); // vypni zobrazovani na posuvnem registru |
if(bit_test(sensors,3)) //...|...// |
{ |
line=S; |
continue; |
} |
if(bit_test(sensors,0)) //|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu |
{ |
line=L3; |
continue; |
} |
if(bit_test(sensors,6)) //......|// |
{ |
line=R3; |
continue; |
} |
if(bit_test(sensors,1)) //.|.....// |
{ |
line=L3; |
continue; |
} |
if(bit_test(sensors,5)) //.....|.// |
{ |
line=R3; |
continue; |
} |
if (bit_test(sensors,2)) //..|....// |
{ |
line=L3; |
continue; |
} |
if (bit_test(sensors,4)) //....|..// |
{ |
line=R3; |
continue; |
} |
} |
} |
//////////////////////////////////////////////////////////////////////////////// |
void objizdka() |
{ |
BL;FR; |
Delay_ms(300); |
FL;BR; |
Delay_ms(100); |
STOPL;STOPR; |
uhel=STRED; |
FL;FR; |
Delay_ms(360); // rovne |
uhel=STRED+55; |
STOPR;FL; |
Delay_ms(190); // doprava |
uhel=STRED; |
FR;FL; |
Delay_ms(300); // rovne |
uhel=STRED+55; |
FL;STOPR; |
Delay_ms(190); // doprava |
uhel=STRED; |
FR;FL; |
Delay_ms(60); // rovne |
While((sensors & 0b11111110)!=0) //dokud neni cara |
{ |
sensors = spi_read(0); // cteni senzoru |
sensors=~sensors; |
Delay_ms(3); // cekani na SLAVE nez pripravi data od cidel |
} |
BL;BR; |
Delay_ms(300); |
uhel=STRED-55; |
FR;STOPL; // doleva |
delay_ms(250); |
line=L3; |
cikcak(); |
} |
/Designs/ROBOTS/3Orbis/SW/objizdka_centrovani_R.BAK |
---|
0,0 → 1,139 |
#define DOLEVA 0 |
#define DOPRAVA 1 |
void cikcak() |
{ |
unsigned int8 i=0; |
uhel=KOLMO1; // aby se dalo tocit na miste |
Delay_ms(100); |
if (line==L2) line=L3; // poznamenej, kde byla cara pred brzdenim |
if (line==S) line=L3; |
if (line==R2) line=R3; |
Delay_ms(3); // prodleva na cteni senzoru pred prenosem |
output_low(STROBE); // zapni zobrazovani na posuvnem registru |
sensors = spi_read(0); // cteni senzoru |
sensors=~sensors; |
output_high(STROBE); // vypni zobrazovani na posuvnem registru |
while(true) |
{ |
Delay_ms(3); |
if (line==L3) |
{ |
GO(L,B,160);GO(R,F,160); |
}; |
if (line==R3) |
{ |
GO(R,B,160);GO(L,F,160); |
}; |
if (line==S) {STOPL;STOPR; i++;} else i=0; |
if (i>=100) break; // pokud je dostatecne dlouho cara vprostred, vypadni |
output_low(STROBE); // zapni zobrazovani na posuvnem registru |
sensors = spi_read(0); // cteni senzoru |
sensors=~sensors; |
output_high(STROBE); // vypni zobrazovani na posuvnem registru |
if(bit_test(sensors,3)) //...|...// |
{ |
line=S; |
continue; |
} |
if(bit_test(sensors,0)) //|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu |
{ |
line=L3; |
continue; |
} |
if(bit_test(sensors,6)) //......|// |
{ |
line=R3; |
continue; |
} |
if(bit_test(sensors,1)) //.|.....// |
{ |
line=L3; |
continue; |
} |
if(bit_test(sensors,5)) //.....|.// |
{ |
line=R3; |
continue; |
} |
if (bit_test(sensors,2)) //..|....// |
{ |
line=L3; |
continue; |
} |
if (bit_test(sensors,4)) //....|..// |
{ |
line=R3; |
continue; |
} |
} |
} |
//////////////////////////////////////////////////////////////////////////////// |
void objizdka() |
{ |
FL;BR; // doprava 90 |
Delay_ms(300); |
BL;FR; // brzdi z toceni |
Delay_ms(100); |
STOPL;STOPR; |
uhel=STRED; |
FL;FR; |
Delay_ms(370); // rovne |
uhel=STRED-55; |
STOPL;FR; |
Delay_ms(190); // doleva |
uhel=STRED; |
FR;FL; |
Delay_ms(300); // rovne |
uhel=STRED-55; |
FR;STOPL; |
Delay_ms(190); // doleva |
uhel=STRED; |
FR;FL; |
Delay_ms(100); // rovne |
While((sensors & 0b11111110)!=0) //dokud neni cara |
{ |
sensors = spi_read(0); // cteni senzoru |
sensors=~sensors; |
Delay_ms(3); // cekani na SLAVE nez pripravi data od cidel |
} |
BL;BR; // zabrzdi |
Delay_ms(250); |
uhel=KOLMO1; |
FL;BR; // doprava |
While((sensors & 0b11111110)!=0) //dokud neni cara |
{ |
sensors = spi_read(0); // cteni senzoru |
sensors=~sensors; |
Delay_ms(3); // cekani na SLAVE nez pripravi data od cidel |
} |
STOPL;STOPR; // zabrzdi |
Delay_ms(300); |
// delay_ms(250); |
line=R3; |
cikcak(); |
} |
/Designs/ROBOTS/Merkur/SW/MerkurU/tank.BAK |
---|
0,0 → 1,268 |
// Program pro predvadeni schopnosti robota Merkur |
//------------------------------------------------ |
#include "tank.h" |
unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru |
unsigned int8 line; // na ktere strane byla detekovana cara |
unsigned int8 speed; // rychlost zataceni |
unsigned int8 last; // kde byla cara, kdyz byly minule cidla mimo |
unsigned int8 rovinka; // pocitadlo pro zjisteni rovneho useku |
int cirkus; // pocitadlo, po kolika akcich se ma delat cirkus |
int1 BW; // urcuje, jestli je cara cerno/bila nebo |
// bilo/cerna (true = bila cara, cerny podklad) |
// Konstanty pro dynamiku pohybu |
#define T_DIRA 120 // po jakem case zataceni se detekuje dira |
#define FW_POMALU 170 // trochu mimo caru vnitrni pas |
#define FW_ZATACKA 200 // rychlost vnejsiho kola pri zataceni |
#define FW_STREDNE 190 // trochu mimo caru vnejsi pas |
#define COUVANI 750 // couvnuti zpet na caru, po detekci diry |
#define MAX_ROVINKA (255-FW_STREDNE) |
#define TRESHOLD 15 // rozhodovaci uroven komparatoru, 0xF = 0.75*Vdd |
#define BUMPER_TRESHOLD 128 // rozhodovaci uroven cidla na prekazku |
//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 (BW != C2OUT) // Senzory na caru |
#define LSENSOR (BW != C1OUT) |
#define BUMPER sAN2 // Senzor na cihlu |
#define DIAG_SERVO PIN_B3 // Propojka pro diagnosticky mod |
#define DIAG_SENSORS PIN_B2 // Propojka pro diagnosticky mod |
#define BARVY PIN_B1 // Propojka pro nastaveni barvy cary |
#define SPEAKER PIN_B0 // vystup pro pipak |
#define LED1 PIN_A4 // LEDky |
#define LED2 PIN_A3 |
#define LED3 PIN_A7 |
#define LED4 PIN_A6 |
// makro pro PWM |
#define GO(motor, direction, power) if(get_timer0()<=power) \ |
{direction##motor;} else {stop##motor;} |
#int_TIMER2 |
void TIMER2_isr() // obsluha zrychlovani |
{ |
if (speed<255) speed++; |
if (rovinka<MAX_ROVINKA) rovinka++; |
} |
// Primitivni Pipani |
void beep(unsigned int16 period, unsigned int16 length) |
{ |
unsigned int16 nn; |
for(nn=length; nn>0; nn--) |
{ |
output_high(SPEAKER); |
delay_us(period); |
output_low(SPEAKER); |
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)) // spusteni diagnostiky cidel |
{ |
if (RSENSOR) beep(1000,200); |
Delay_ms(300); |
if (LSENSOR) beep(2000,300); |
Delay_ms(300); |
if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD)) beep(3000,400); |
Delay_ms(300); |
}; |
} |
/////////////////////////////////////////////////////////////////////////////// |
void OtocSe() // otoci se zpet, kdyz je prekazka |
{ |
unsigned int16 n; |
BL; BR; // cukni zpatky |
Delay_ms(200); |
STOPR;STOPL; |
beep(800,400); |
beep(2000,1000); |
output_low(LED4); |
beep(900,400); |
output_low(LED1); |
BR; FL; Delay_ms(100); // otoc se 30° do prava |
STOPL; STOPR; |
beep(1000,1000); |
output_low(LED3); |
BR; FL; |
for(n=40000;n>0;n--) // toc se, dokud nenarazis na caru |
{ |
line = RSENSOR; // cteni senzoru na caru |
line |= LSENSOR << 1; |
if (line!=0) break; |
} |
STOPR; STOPL; |
output_high(LED1); output_high(LED3); output_high(LED4); |
line=L; // caru jsme prejeli, tak je vlevo |
cirkus=0; |
} |
void main() |
{ |
unsigned int16 n; // pro FOR |
unsigned int16 i; |
STOPL; STOPR; // prepne vystupy na ovladani motoru na output a zastavi |
setup_oscillator(OSC_4MHZ|OSC_INTRC); // 4 MHz interni RC oscilator |
port_b_pullups(TRUE); // pullups pro piano na diagnostiku |
setup_spi(FALSE); |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro PWM |
setup_timer_2(T2_DIV_BY_4,255,10); // Casovac pro regulaci |
// preruseni kazdych 10ms |
setup_adc_ports(BUMPER|VSS_VDD); // nastaveni A/D prevodniku pro naraznik |
setup_adc(ADC_CLOCK_INTERNAL); |
set_adc_channel(2); |
setup_timer_1(T1_INTERNAL|T1_DIV_BY_1); // Casovac pro naraznik |
setup_ccp1(CCP_COMPARE_RESET_TIMER); |
CCP_1=(2^10)-1; // prevod kazdou 1ms |
output_low(LED1); output_low(LED2); output_low(LED3); output_low(LED4); |
setup_comparator(A0_VR_A1_VR); // inicializace komparatoru pro cidla cary |
setup_vref(VREF_HIGH|TRESHOLD); // 32 kroku od 0.25 do 0.75 Vdd |
Beep(1000,200); //double beep |
Delay_ms(50); |
Beep(1000,200); |
Delay_ms(1000); // 1s |
// povoleni rizeni rychlosti zataceni pres preruseni |
enable_interrupts(INT_TIMER2); |
enable_interrupts(GLOBAL); |
/*---------------------------------------------------------------------------*/ |
sensors=S; |
line=S; |
last=S; |
cirkus=0; |
// movement=S; |
speed=FW_POMALU; |
BW=input(BARVY); // Jaka ma byt barva cary? |
diagnostika(); // Zkus, jestli nekdo nechce, diagnostiku |
Delay_ms(500); |
output_high(LED1); Beep(1000,200); Delay_ms(500); |
output_high(LED2); Beep(1000,200); Delay_ms(500); |
output_high(LED3); Beep(1000,200); Delay_ms(500); |
output_high(LED4); Beep(1000,200); Delay_ms(500); |
while(true) // hlavni smycka (jizda podle cary) |
{ |
sensors = RSENSOR; // cteni senzoru na caru |
sensors |= LSENSOR << 1; |
if (read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD) OtocSe(); |
switch (sensors) // zatacej podle toho, kde vidis caru |
{ |
case S: // rovne |
GO(L, F, FW_STREDNE+rovinka); GO(R, F, FW_STREDNE+rovinka); |
continue; |
case L: // trochu vlevo |
GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka); |
line=L; |
continue; |
case R: // trochu vpravo |
GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka); |
line=R; |
continue; |
default: // kdyz jsou obe cidla mimo caru, tak pokracuj dal |
}; |
rovinka=0; |
if (last!=line) // pokud si prejel caru z jedne strany na druhou stranu, |
// tak zabrzdi |
{ |
output_bit(LED1, !input(LED1)); |
last=line; |
speed=FW_ZATACKA; |
cirkus++; |
if (cirkus>8) |
{ |
STOPL; STOPR; |
cirkus=0; |
disable_interrupts(GLOBAL); |
beep(1000,400); |
for(n=3000; n>3950; n--) beep(n,10); |
output_low(LED1); |
beep(2000,200); |
beep(900,400); |
for(n=2950; n<3000; n++) beep(n,10); |
output_low(LED2); |
output_high(LED1); |
beep(4000,400); |
beep(1000,100); |
output_low(LED3); |
beep(3000,400); |
Delay_ms(1000); |
output_high(LED1); output_high(LED2); |
output_high(LED3); output_high(LED4); |
enable_interrupts(GLOBAL); |
} |
}; |
if (L==line) // kdyz jsou obe cidla mimo caru, zatoc na caru |
{ |
STOPL; |
GO(R, F, speed); |
} |
else |
{ |
STOPR; |
GO(L, F, speed); |
} |
} // while(true) |
} |
/Designs/ROBOTS/Merkur/SW/turn_L/tank.BAK |
---|
0,0 → 1,340 |
#include "tank.h" |
#define DEBUG |
#define TXo PIN_A3 // To the transmitter modulator |
#include "AX25.c" // podprogram pro prenos telemetrie |
unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru |
unsigned int8 line; // na ktere strane byla detekovana cara |
unsigned int8 speed; // rychlost zataceni |
unsigned int8 rovinka; // pocitadlo pro zjisteni rovneho useku |
unsigned int8 last; // kde byla cara, kdyz byly minule cidla mimo |
unsigned int8 movement; // obsahuje aktualni smer zataceni |
unsigned int8 dira; // pocita dobu po kterou je ztracena cara |
// Konstanty pro dynamiku pohybu |
#define T_DIRA 87 // po jakem case zataceni se detekuje dira |
#define INC_SPEED 1 // prirustek rychlosti v jednom kroku |
#define FW_POMALU 230 // trochu mimo caru vnitrni pas |
#define FW_ZATACKA 200 // rychlost vnejsiho kola pri zataceni |
#define FW_STREDNE 240 // trochu mimo caru vnejsi pas |
#define COUVANI 600 // couvnuti zpet na caru, po detekci diry |
#define PRES_DIRU 250 |
#define MAX_ROVINKA (255-FW_STREDNE) |
#define TRESHOLD 10 // rozhodovaci uroven komparatoru, 0xF = 0.75*Vdd |
#define BUMPER_TRESHOLD 128 |
#define CIK_CAK 30000 |
#define T_CIHLA 50 // perioda detekce cihly |
//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 C2OUT // Senzory na caru |
#define LSENSOR C1OUT |
#define BUMPER PIN_A4 // Senzor na cihlu |
#define DIAG_SERVO PIN_B3 // Propojka pro diagnosticky mod |
#define DIAG_SENSORS PIN_B2 // Propojka pro diagnosticky mod |
#DEFINE SOUND_HI PIN_A6 // komplementarni vystupy pro piezo pipak |
#DEFINE SOUND_LO PIN_A7 |
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_TIMER2 |
void TIMER2_isr() |
{ |
if (speed<255) speed+=INC_SPEED; |
if (rovinka<MAX_ROVINKA) rovinka++; |
if (dira<=T_DIRA) dira++; |
} |
// 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)) |
{ |
if (RSENSOR) beep(900,500); |
if (LSENSOR) beep(800,500); |
if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD)) beep(1000,500); |
}; |
} |
/////////////////////////////////////////////////////////////////////////////// |
void cikcak() |
{ |
unsigned int16 n; |
sem1: |
n=CIK_CAK; |
while (0==RSENSOR||LSENSOR) // zkontroluj caru |
{ |
if (n==CIK_CAK) // zmen smer zataceni |
{ |
n=0; |
switch(movement) |
{ |
case L: |
FL;BR; |
movement=R; |
break; |
case R: |
FR;BL; |
movement=L; |
break; |
case S: |
FL;BR; |
movement=R; |
n=CIK_CAK/2; |
break; |
} |
} |
n++; |
} |
STOPL;STOPR; |
line = RSENSOR; // cteni senzoru na caru |
line |= LSENSOR << 1; |
if (line==0) goto sem1; |
// nasli jsme caru |
line=S; |
} |
/////////////////////////////////////////////////////////////////////////////// |
void objizdka() // objede cihlu |
{ |
unsigned int16 n; |
BL;BR;Delay_ms(200); |
STOPR;STOPL; |
beep(900,1000); |
// movement=S; |
//cikcak(); |
BL; FR; Delay_ms(215); // otoc se 70° do leva |
FR; FL; Delay_ms(600); // popojed rovne |
BR; Delay_ms(50); // otoc se 90° do prava |
STOPR; FL; Delay_ms(700); |
FR; FL; Delay_ms(100); // popojed rovne na slepo |
for(n=600;n>0;n--) // popojed rovne ale kontroluj caru |
{ |
line = RSENSOR; // cteni senzoru na caru |
line |= LSENSOR << 1; |
if (line!=0) {Delay_ms(50); break;} // kdyz narazis na caru, za chvili zastav |
Delay_ms(1); |
} |
BL; // otoc se 60° do leva |
for(n=600;n>0;n--) |
{ |
line = RSENSOR; // cteni senzoru na caru |
line |= LSENSOR << 1; |
if (line!=0) break; |
Delay_ms(1); |
} |
STOPR; STOPL; |
movement=R; |
cikcak(); |
dira=0; |
} |
/////////////////////////////////////////////////////////////////////////////// |
void prejeddiru() // vyresi diru |
{ |
unsigned int16 n; |
unsigned int8 speed_dira; |
STOPL;STOPR; |
speed_dira=speed; |
beep(1000,500); |
switch (movement) //vrat se zpet na caru |
{ |
case L: |
for (n=COUVANI;n>0;n--) {GO(R,B,speed_dira); Delay_ms(1);} |
STOPL;STOPR; |
break; |
case R: |
for (n=COUVANI;n>0;n--) {GO(L,B,speed_dira); Delay_ms(1);} |
STOPL;STOPR; |
break; |
case S: |
goto sem; |
break; |
} |
beep(800,500); |
line=0; |
FR; BL; Delay_ms(300); // otoc se na caru |
while(line==0) |
{ |
line = RSENSOR; // cteni senzoru na caru |
line |= LSENSOR << 1; |
} |
FL;BR; Delay_ms(60); |
STOPL; STOPR; |
FL; BR; Delay_ms(500); |
STOPL; STOPR; |
Delay_ms(1000); |
FR;FL; //popojed rovne |
for(n=PRES_DIRU;n>0;n--) |
{ |
line = RSENSOR; // cteni senzoru na caru |
line |= LSENSOR << 1; |
if (line!=0) break; |
Delay_ms(1); |
} |
sem: |
STOPL; STOPR; |
movement=S; |
cikcak(); // najdi caru |
dira=0; |
} |
/////////////////////////////////////////////////////////////////////////////// |
void main() |
{ |
unsigned int16 n; // pro FOR |
STOPL; STOPR; // prepne vystupy na ovladani motoru na output a zastavi |
setup_oscillator(OSC_4MHZ|OSC_INTRC); // 4 MHz interni RC oscilator |
port_b_pullups(TRUE); // pullups pro piano na diagnostiku |
setup_spi(FALSE); |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro PWM |
setup_timer_2(T2_DIV_BY_4,255,10); // Casovac pro regulaci |
// preruseni kazdych 10ms |
setup_adc_ports(sAN2|VSS_VDD); // nastaveni A/D prevodniku pro naraznik |
setup_adc(ADC_CLOCK_INTERNAL); |
set_adc_channel(2); |
setup_timer_1(T1_INTERNAL|T1_DIV_BY_1); // Casovac pro naraznik |
setup_ccp1(CCP_COMPARE_RESET_TIMER); |
CCP_1=(2^10)-1; // prevod kazdou 1ms |
setup_comparator(A0_VR_A1_VR); // inicializace komparatoru pro cidla cary |
setup_vref(VREF_HIGH|TRESHOLD); // 32 kroku od 0.25 do 0.75 Vdd |
Beep(1000,200); //double beep |
Delay_ms(50); |
Beep(1000,200); |
Delay_ms(1000); // 1s |
// povoleni rizeni rychlosti zataceni pres preruseni |
enable_interrupts(INT_TIMER2); |
enable_interrupts(GLOBAL); |
/*---------------------------------------------------------------------------*/ |
sensors=S; |
line=S; |
last=S; |
movement=S; |
speed=FW_POMALU; |
diagnostika(); |
// cikcak(); // toc se, abys nasel caru |
Delay_ms(500); |
Beep(1000,200); |
Delay_ms(500); |
while(true) // hlavni smycka (jizda podle cary) |
{ |
sensors = RSENSOR; // cteni senzoru na caru |
sensors |= LSENSOR << 1; |
if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD) && (dira<=T_CIHLA)) objizdka(); |
switch (sensors) // zatacej podle toho, kde vidis caru |
{ |
case S: // rovne |
FL; FR; // pokud se jede dlouho rovne, tak pridej |
dira=0; |
movement=S; |
continue; |
case L: // trochu vlevo |
GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka); |
line=L; |
dira=0; |
movement=L; |
continue; |
case R: // trochu vpravo |
GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka); |
line=R; |
dira=0; |
movement=R; |
continue; |
default: // kdyz jsou obe cidla mimo caru, tak pokracuj dal |
} |
rovinka=0; |
if (dira>=T_DIRA) prejeddiru(); |
if (last!=line) // pokud si prejel caru z jedne strany na druhou stranu, tak zabrzdi |
{ |
last=line; |
speed=FW_ZATACKA; |
} |
if (L==line) // kdyz jsou obe cidla mimo caru, zatoc na caru |
{ |
STOPL; |
GO(R, F, speed); |
movement=L; |
} |
else |
{ |
STOPR; |
GO(L, F, speed); |
movement=R; |
} |
} // while(true) |
} |
/Designs/ROBOTS/Merkur/SW/turn_R/tank.BAK |
---|
0,0 → 1,339 |
#include "tank.h" |
#define DEBUG |
#define TXo PIN_A3 // To the transmitter modulator |
#include "AX25.c" // podprogram pro prenos telemetrie |
unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru |
unsigned int8 line; // na ktere strane byla detekovana cara |
unsigned int8 speed; // rychlost zataceni |
unsigned int8 rovinka; // pocitadlo pro zjisteni rovneho useku |
unsigned int8 last; // kde byla cara, kdyz byly minule cidla mimo |
unsigned int8 movement; // obsahuje aktualni smer zataceni |
unsigned int8 dira; // pocita dobu po kterou je ztracena cara |
// Konstanty pro dynamiku pohybu |
#define T_DIRA 120 // po jakem case zataceni se detekuje dira |
#define INC_SPEED 1 // prirustek rychlosti v jednom kroku |
#define FW_POMALU 230 // trochu mimo caru vnitrni pas |
#define FW_ZATACKA 240 // rychlost vnejsiho kola pri zataceni |
#define FW_STREDNE 240 // trochu mimo caru vnejsi pas |
#define COUVANI 750 // couvnuti zpet na caru, po detekci diry |
#define PRES_DIRU 300 |
#define MAX_ROVINKA (255-FW_STREDNE) |
#define TRESHOLD 6 // rozhodovaci uroven komparatoru, 0xF = 0.75*Vdd |
#define BUMPER_TRESHOLD 128 |
#define CIK_CAK 30000 |
#define T_CIHLA 50 // perioda detekce cihly |
//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 C2OUT // Senzory na caru |
#define LSENSOR C1OUT |
#define BUMPER PIN_A4 // Senzor na cihlu |
#define DIAG_SERVO PIN_B3 // Propojka pro diagnosticky mod |
#define DIAG_SENSORS PIN_B2 // Propojka pro diagnosticky mod |
#DEFINE SOUND_HI PIN_A6 // komplementarni vystupy pro piezo pipak |
#DEFINE SOUND_LO PIN_A7 |
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_TIMER2 |
void TIMER2_isr() |
{ |
if (speed<255) speed+=INC_SPEED; |
if (rovinka<MAX_ROVINKA) rovinka++; |
if (dira<=T_DIRA) dira++; |
} |
// 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)) // spusteni diagnostiky cidel |
{ |
if (RSENSOR) beep(900,500); |
if (LSENSOR) beep(800,500); |
if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD)) beep(1000,500); |
}; |
} |
/////////////////////////////////////////////////////////////////////////////// |
void cikcak() |
{ |
unsigned int16 n; |
sem1: |
n=CIK_CAK; |
while (0==RSENSOR||LSENSOR) // zkontroluj caru |
{ |
if (n==CIK_CAK) // zmen smer zataceni |
{ |
n=0; |
switch(movement) |
{ |
case L: |
FL;BR; |
movement=R; |
break; |
case R: |
FR;BL; |
movement=L; |
break; |
case S: |
FL;BR; |
movement=R; |
n=CIK_CAK/2; |
break; |
} |
} |
n++; |
} |
STOPL;STOPR; |
line = RSENSOR; // cteni senzoru na caru |
line |= LSENSOR << 1; |
if (line==0) goto sem1; |
// nasli jsme caru |
line=S; |
} |
/////////////////////////////////////////////////////////////////////////////// |
void objizdka() // objede cihlu |
{ |
unsigned int16 n; |
BL;BR;Delay_ms(150); |
STOPR;STOPL; |
beep(900,1000); |
// movement=S; |
// cikcak(); |
BR; FL; Delay_ms(270); // otoc se 70° do prava |
FR; FL; Delay_ms(700); // popojed rovne |
BL; Delay_ms(30); // otoc se 90° do leva |
STOPL; FR; Delay_ms(500); |
FR; FL; Delay_ms(100); // popojed rovne na slepo |
for(n=40000;n>0;n--) // popojed rovne ale kontroluj caru |
{ |
line = RSENSOR; // cteni senzoru na caru |
line |= LSENSOR << 1; |
if (line!=0) {Delay_ms(150); break;} |
// Delay_ms(1); |
} |
BR; FL; // otoc se 60° do prava |
for(n=40000;n>0;n--) |
{ |
line = RSENSOR; // cteni senzoru na caru |
line |= LSENSOR << 1; |
if (line!=0) break; |
// Delay_ms(1); |
} |
STOPR; STOPL; |
movement=L; //R; |
cikcak(); |
dira=0; |
} |
/////////////////////////////////////////////////////////////////////////////// |
void prejeddiru() // vyresi diru |
{ |
unsigned int16 n; |
unsigned int8 speed_dira; |
STOPL;STOPR; |
speed_dira=speed; |
beep(1000,500); |
switch (movement) //vrat se zpet na caru |
{ |
case L: |
for (n=COUVANI;n>0;n--) {GO(R,B,speed_dira); Delay_ms(1);} |
STOPL;STOPR; |
break; |
case R: |
for (n=COUVANI;n>0;n--) {GO(L,B,speed_dira); Delay_ms(1);} |
STOPL;STOPR; |
break; |
case S: |
goto sem; |
break; |
} |
beep(1000,500); |
/*line=0; |
FR; BL; Delay_ms(400); // otoc se na caru |
beep(1000,500); |
while(line==0) |
{ |
line = RSENSOR; // cteni senzoru na caru |
line |= LSENSOR << 1; |
} |
FL;BR; Delay_ms(60); // zabrzdi |
STOPL; STOPR; |
FL; BR; Delay_ms(700); // otacka 180 deg |
STOPL; STOPR;*/ |
FR;FL; //popojed rovne |
for(n=PRES_DIRU;n>0;n--) |
{ |
line = RSENSOR; // cteni senzoru na caru |
line |= LSENSOR << 1; |
if (line!=0) break; |
Delay_ms(1); |
} |
sem: |
STOPL; STOPR; |
movement=S; |
cikcak(); // najdi caru |
dira=0; |
} |
/////////////////////////////////////////////////////////////////////////////// |
void main() |
{ |
unsigned int16 n; // pro FOR |
STOPL; STOPR; // prepne vystupy na ovladani motoru na output a zastavi |
setup_oscillator(OSC_4MHZ|OSC_INTRC); // 4 MHz interni RC oscilator |
port_b_pullups(TRUE); // pullups pro piano na diagnostiku |
setup_spi(FALSE); |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro PWM |
setup_timer_2(T2_DIV_BY_4,255,10); // Casovac pro regulaci |
// preruseni kazdych 10ms |
setup_adc_ports(sAN2|VSS_VDD); // nastaveni A/D prevodniku pro naraznik |
setup_adc(ADC_CLOCK_INTERNAL); |
set_adc_channel(2); |
setup_timer_1(T1_INTERNAL|T1_DIV_BY_1); // Casovac pro naraznik |
setup_ccp1(CCP_COMPARE_RESET_TIMER); |
CCP_1=(2^10)-1; // prevod kazdou 1ms |
setup_comparator(A0_VR_A1_VR); // inicializace komparatoru pro cidla cary |
setup_vref(VREF_HIGH|TRESHOLD); // 32 kroku od 0.25 do 0.75 Vdd |
Beep(1000,200); //double beep |
Delay_ms(50); |
Beep(1000,200); |
Delay_ms(1000); // 1s |
// povoleni rizeni rychlosti zataceni pres preruseni |
enable_interrupts(INT_TIMER2); |
enable_interrupts(GLOBAL); |
/*---------------------------------------------------------------------------*/ |
sensors=S; |
line=S; |
last=S; |
movement=S; |
speed=FW_POMALU; |
diagnostika(); |
//cikcak(); // toc se, abys nasel caru |
Delay_ms(500); |
Beep(1000,200); |
Delay_ms(500); |
while(true) // hlavni smycka (jizda podle cary) |
{ |
sensors = RSENSOR; // cteni senzoru na caru |
sensors |= LSENSOR << 1; |
if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD) && (dira<=T_CIHLA)) objizdka(); |
switch (sensors) // zatacej podle toho, kde vidis caru |
{ |
case S: // rovne |
FL; FR; // pokud se jede dlouho rovne, tak pridej |
dira=0; |
movement=S; |
continue; |
case L: // trochu vlevo |
GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka); |
line=L; |
dira=0; |
movement=L; |
continue; |
case R: // trochu vpravo |
GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka); |
line=R; |
dira=0; |
movement=R; |
continue; |
default: // kdyz jsou obe cidla mimo caru, tak pokracuj dal |
} |
rovinka=0; |
if (dira>=T_DIRA) prejeddiru(); |
if (last!=line) // pokud si prejel caru z jedne strany na druhou stranu, tak zabrzdi |
{ |
last=line; |
speed=FW_ZATACKA; |
} |
if (L==line) // kdyz jsou obe cidla mimo caru, zatoc na caru |
{ |
STOPL; |
GO(R, F, speed); |
movement=L; |
} |
else |
{ |
STOPR; |
GO(L, F, speed); |
movement=R; |
} |
} // while(true) |
} |
/Designs/ROBOTS/Merkur/SW/vystava/tank.BAK |
---|
0,0 → 1,236 |
#include "tank.h" |
unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru |
unsigned int8 line; // na ktere strane byla detekovana cara |
unsigned int8 speed; // rychlost zataceni |
unsigned int8 last; // kde byla cara, kdyz byly minule cidla mimo |
unsigned int8 rovinka; // pocitadlo pro zjisteni rovneho useku |
int cirkus; |
// Konstanty pro dynamiku pohybu |
#define T_DIRA 120 // po jakem case zataceni se detekuje dira |
#define FW_POMALU 170 // trochu mimo caru vnitrni pas |
#define FW_ZATACKA 200 // rychlost vnejsiho kola pri zataceni |
#define FW_STREDNE 190 // trochu mimo caru vnejsi pas |
#define COUVANI 750 // couvnuti zpet na caru, po detekci diry |
#define MAX_ROVINKA (255-FW_STREDNE) |
#define TRESHOLD 15 // rozhodovaci uroven komparatoru, 0xF = 0.75*Vdd |
#define BUMPER_TRESHOLD 128 |
//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 !C2OUT // Senzory na caru |
#define LSENSOR !C1OUT |
#define BUMPER PIN_A4 // Senzor na cihlu |
#define DIAG_SERVO PIN_B3 // Propojka pro diagnosticky mod |
#define DIAG_SENSORS PIN_B2 // Propojka pro diagnosticky mod |
#DEFINE SOUND_HI PIN_A6 // komplementarni vystupy pro piezo pipak |
#DEFINE SOUND_LO PIN_A7 |
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_TIMER2 |
void TIMER2_isr() |
{ |
if (speed<255) speed++; |
if (rovinka<MAX_ROVINKA) rovinka++; |
} |
// 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)) // spusteni diagnostiky cidel |
{ |
if (RSENSOR) beep(1000,1000); |
if (LSENSOR) beep(2000,2000); |
if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD)) beep(3000,3000); |
}; |
} |
/////////////////////////////////////////////////////////////////////////////// |
void OtocSe() // otoci se zpet, kdyz je prekazka |
{ |
unsigned int16 n; |
STOPR;STOPL; |
beep(800,400); |
beep(2000,1000); |
beep(900,400); |
BR; FL; Delay_ms(100); // otoc se 30° do prava |
STOPL; STOPR; |
beep(1000,1000); |
BR; FL; |
for(n=40000;n>0;n--) // toc se, dokud nenarazis na caru |
{ |
line = RSENSOR; // cteni senzoru na caru |
line |= LSENSOR << 1; |
if (line!=0) break; |
} |
STOPR; STOPL; |
line=L; // caru jsme prejeli, tak je vlevo |
cirkus=0; |
} |
void main() |
{ |
unsigned int16 n; // pro FOR |
unsigned int16 i; |
STOPL; STOPR; // prepne vystupy na ovladani motoru na output a zastavi |
setup_oscillator(OSC_4MHZ|OSC_INTRC); // 4 MHz interni RC oscilator |
port_b_pullups(TRUE); // pullups pro piano na diagnostiku |
setup_spi(FALSE); |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro PWM |
setup_timer_2(T2_DIV_BY_4,255,10); // Casovac pro regulaci |
// preruseni kazdych 10ms |
setup_adc_ports(sAN2|VSS_VDD); // nastaveni A/D prevodniku pro naraznik |
setup_adc(ADC_CLOCK_INTERNAL); |
set_adc_channel(2); |
setup_timer_1(T1_INTERNAL|T1_DIV_BY_1); // Casovac pro naraznik |
setup_ccp1(CCP_COMPARE_RESET_TIMER); |
CCP_1=(2^10)-1; // prevod kazdou 1ms |
setup_comparator(A0_VR_A1_VR); // inicializace komparatoru pro cidla cary |
setup_vref(VREF_HIGH|TRESHOLD); // 32 kroku od 0.25 do 0.75 Vdd |
Beep(1000,200); //double beep |
Delay_ms(50); |
Beep(1000,200); |
Delay_ms(1000); // 1s |
// povoleni rizeni rychlosti zataceni pres preruseni |
enable_interrupts(INT_TIMER2); |
enable_interrupts(GLOBAL); |
/*---------------------------------------------------------------------------*/ |
sensors=S; |
line=S; |
last=S; |
cirkus=0; |
// movement=S; |
speed=FW_POMALU; |
diagnostika(); |
Delay_ms(500); |
Beep(1000,200); |
Delay_ms(500); |
while(true) // hlavni smycka (jizda podle cary) |
{ |
sensors = RSENSOR; // cteni senzoru na caru |
sensors |= LSENSOR << 1; |
if (read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD) OtocSe(); |
switch (sensors) // zatacej podle toho, kde vidis caru |
{ |
case S: // rovne |
GO(L, F, FW_STREDNE+rovinka); GO(R, F, FW_STREDNE+rovinka); |
continue; |
case L: // trochu vlevo |
GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka); |
line=L; |
continue; |
case R: // trochu vpravo |
GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka); |
line=R; |
continue; |
default: // kdyz jsou obe cidla mimo caru, tak pokracuj dal |
}; |
rovinka=0; |
if (last!=line) // pokud si prejel caru z jedne strany na druhou stranu, tak zabrzdi |
{ |
last=line; |
speed=FW_ZATACKA; |
cirkus++; |
if (cirkus>10) |
{ |
STOPL; STOPR; |
cirkus=0; |
disable_interrupts(GLOBAL); |
beep(1000,400); |
for(n=3000; n>3950; n--) beep(n,10); |
beep(2000,200); |
beep(900,400); |
for(n=2950; n<3000; n++) beep(n,10); |
beep(4000,400); |
beep(1000,100); |
beep(3000,400); |
Delay_ms(1000); |
enable_interrupts(GLOBAL); |
} |
}; |
if (L==line) // kdyz jsou obe cidla mimo caru, zatoc na caru |
{ |
STOPL; |
GO(R, F, speed); |
} |
else |
{ |
STOPR; |
GO(L, F, speed); |
} |
} // while(true) |
} |