#define DEBUG

C:\dokumenty\@Dokumenty\Roboti\IstRobot2005\FW\Tank\tank.c

25-IV-05 Page 12:45 1 of am8

#include "tank.h" / /#define DEBUG #define TXo PIN_B7 #include "AX25.c" unsigned unsigned unsigned unsigned unsigned

int8 int8 int8 int8 int8

/ / To the transmitter modulator / / podprogram pro prenos telemetrie

sensors; / / pomocna promenna pro cteni cidel na caru line; / / na ktere strane byla detekovana cara dira; / / pocitadlo pro nalezeni preruseni cary speed; / / rychlost zataceni straight; / / pocitadlo pro zjisteni rovneho useku

/ / Konstanty pro dynamiku pohybu #define T_DIRA 14 / / po jakem case zataceni se detekuje dira #define INC_SPEED 5 / / prirustek rychlosti v jednom kroku #define RIGHT_ANGLE 8000 / / 90 stupnu #define CIKCAK 8000 / / 45 stupnu #define CIK 5000 / / setrvacnost po cik #define BW_PO_DIRE 13000 / / 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 120 / / minimalni rychlost pri zataceni #define TURN_MAX 170 / / miximalni rychlost pri zataceni #define POPOJED 4000 / / popojeti rovne pro cik-cak #define BRZDENI 1000 / / zpetny chod, aby se pas zastavil po rovince #define ROVINKA 8 / / doba po kterou se musi jet rovne, aby se brzdilo #define TRESHOLD 15 / / rozhodovaci uroven komparatoru, 0xF = 0.75*Vdd #define B_DIST 50 / / vzdalenost brzdeni pred cihlou #define DIAG_SLOW 100 / / pomalu pro diagnostiku / /motory #define FR #define FL #define BR #define BL #define STOPR #define STOPL #define L 0b10 #define R 0b01 #define S 0b11

//Napred vypnout potom zapnout! output_low(PIN_B2); output_high(PIN_B3) output_low(PIN_B0); output_high(PIN_B1) output_low(PIN_B3); output_high(PIN_B2) output_low(PIN_B1); output_high(PIN_B0) output_low(PIN_B3);output_low(PIN_B2) output_low(PIN_B1);output_low(PIN_B0) / / left / / right / / straight

/ /cidla #define RSENSOR #define LSENSOR #define BUMPER

C2OUT C1OUT PIN_A4

#define DIAG_SERVO #define DIAG_SENSORS #DEFINE SOUND_HI #DEFINE SOUND_LO char AXstring[40];

PIN_B4 PIN_B5

PIN_A6 PIN_A7

/ / Vpred / / Vzad / / Zastav

/ / Senzory na caru / / Senzor na cihlu / / Propojka pro diagnosticky mod / / Propojka pro diagnosticky mod / / komplementarni vystupy pro piezo pipak

/ / 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() {

/ / This function is called every time / / the RTCC (timer0) overflows (255->0). / / For this program this is apx 76 times / / per second. if (speed
C:\dokumenty\@Dokumenty\Roboti\IstRobot2005\FW\Tank\tank.c // }

25-IV-05 Page 12:45 2 of am8

read_adc(ADC_START_ONLY);

/ / 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 cik_cak() / / Hledani cary { unsigned int16 n; while(true) { FL; FR; / / kousek vpred for (n=POPOJED; n>0; n--) / / prodleva { sensors = RSENSOR; / / cteni senzoru na caru sensors |= LSENSOR << 1; / / pokud se pri brzdeni narazi na caru if(sensors!=0) { line=sensors; / / poznamenej to goto vpred; }; }; STOPL; STOPR; for(n=CIK ;n>0 ;n--) if (1==(LSENSOR|RSENSOR)) goto vpred; / / je cara ? //

FL; BR; // vpravo line=L; / / v pripade prejeti je cara na druhe strane for(n=CIKCAK ;n>0 ;n--) { GO(L, F, speed); GO(R, B, speed); if (1==(LSENSOR|RSENSOR)) goto vpred; / / je cara ? }; STOPL; STOPR; for(n=CIK ;n>0 ;n--) if (1==(LSENSOR|RSENSOR)) goto vpred; / / je cara ?

//

BL; FR; // vlevo line=R; / / v pripade prejeti je cara na druhe strane for(n=CIKCAK ;n>0 ;n--) { GO(L, B, speed); GO(R, F, speed); if (1==(LSENSOR|RSENSOR)) goto vpred; / / je cara ? }; STOPL; STOPR; for(n=CIK ;n>0 ;n--) if (1==(LSENSOR|RSENSOR)) goto vpred; / / je cara ?

//

BL; FR; // vlevo line=R; for(n=CIKCAK ;n>0 ;n--) { GO(L, B, speed); GO(R, F, speed); if (1==(LSENSOR|RSENSOR)) goto vpred; / / je cara ? }; STOPL; STOPR; for(n=CIK ;n>0 ;n--) if (1==(LSENSOR|RSENSOR)) goto vpred;

/ / je cara ?

C:\dokumenty\@Dokumenty\Roboti\IstRobot2005\FW\Tank\tank.c //

25-IV-05 Page 12:45 3 of am8

FL; BR; // vpravo line=L; / / v pripade prejeti je cara na druhe strane for(n=CIKCAK ;n>0 ;n--) { GO(L, F, speed); GO(R, B, speed); if (1==(LSENSOR|RSENSOR)) goto vpred; / / je cara ? }; STOPL; STOPR; for(n=CIK ;n>0 ;n--) if (1==(LSENSOR|RSENSOR)) goto vpred; / / je cara ? };

vpred: STOPL; STOPR; dira=0; straight=0; speed=255; } / / Diagnostika pohonu a cidel, hejbne vsema motorama ve vsech smerech void diagnostika() { unsigned int16 n; while (input(DIAG_SERVO)) / / Propojka, ktera spousti diagnostiku { / / zastav vse STOPL; STOPR; sprintf(AXstring,"Start diagnostiky...\0"); / / Convert DATA to String. SendPacket(&AXstring[0]); for (n=500; n<800; n+=100) { beep(n,n); / /beep UP }; Delay_ms(1000); / / vpred for (n=50000; n>0; n--) { GO(L, F, DIAG_SLOW); GO(R, F, DIAG_SLOW); }; STOPR; STOPL; sprintf(AXstring,"Sensor: %U\0", read_adc()); SendPacket(&AXstring[0]); Beep(880,100); Delay_ms(1000); / / vzad for (n=50000; n>0; n--) { GO(L, B, DIAG_SLOW); GO(R, B, DIAG_SLOW); }; STOPR; STOPL; sprintf(AXstring,"Sensor: %U\0", read_adc()); SendPacket(&AXstring[0]); Beep(880,100); Delay_ms(1000); / / vlevo for (n=50000; n>0; n--) { GO(L, B, DIAG_SLOW); GO(R, F, DIAG_SLOW); }; STOPR; STOPL; sprintf(AXstring,"Sensor: %U\0", read_adc()); SendPacket(&AXstring[0]); Beep(880,100); Delay_ms(1000);

/ / Convert DATA to String.



C:\dokumenty\@Dokumenty\Roboti\IstRobot2005\FW\Tank\tank.c / / vpravo for (n=50000; n>0; n--) { GO(L, F, DIAG_SLOW); GO(R, B, DIAG_SLOW); }; STOPR; STOPL; sprintf(AXstring,"Sensor: %U\0", read_adc()); SendPacket(&AXstring[0]); Beep(880,100); Delay_ms(1000);

25-IV-05 Page 12:45 4 of am8


}; while (input(DIAG_SENSORS)) / / vysilani telemetrie { setup_adc_ports(NO_ANALOGS|VSS_VDD); setup_adc(ADC_OFF); 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 sprintf(AXstring,"L: %U R: %U\0", C1OUT, C2OUT); / / Convert DATA to String. SendPacket(&AXstring[0]); Delay_ms(1000); setup_comparator(NC_NC_NC_NC); setup_adc_ports(sAN0|sAN1|VSS_VDD); setup_adc(ADC_CLOCK_INTERNAL); set_adc_channel(1); Delay_ms(100); sprintf(AXstring,"R_AD: %U\0", read_adc()); / / Convert DATA to String. SendPacket(&AXstring[0]); Delay_ms(1000); set_adc_channel(0); Delay_ms(100); sprintf(AXstring,"L_AD: %U\0", read_adc()); / / Convert DATA to String. SendPacket(&AXstring[0]); Delay_ms(1000); set_adc_channel(2); Delay_ms(100); sprintf(AXstring,"GP2D120: %U\0", read_adc()); / / Convert DATA to String. SendPacket(&AXstring[0]); delay_ms(500); }; }

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_1(T1_INTERNAL|T1_DIV_BY_1); // Casovac pro naraznik setup_timer_1(T1_DISABLED); setup_timer_2(T2_DIV_BY_16,255,16);

/ / Casovac pro regulaci

diagnostika(); setup_adc_ports(sAN2|VSS_VDD); / / nastaveni A/D prevodniku setup_adc(ADC_CLOCK_INTERNAL); set_adc_channel(2); // setup_ccp1(CCP_COMPARE_RESET_TIMER); // CCP_1=(2^10)-1; // prevod kazdou 1ms setup_ccp1(CCP_OFF); setup_comparator(A0_VR_A1_VR);

/ / inicializace komparatoru pro cidla cary

C:\dokumenty\@Dokumenty\Roboti\IstRobot2005\FW\Tank\tank.c setup_vref(VREF_HIGH|TRESHOLD); Beep(1000,200); Delay_ms(50); Beep(1000,200); Delay_ms(1000);

25-IV-05 Page 12:45 5 of am8

/ / 32 kroku od 0.25 do 0.75 Vdd

/ /double beep

/ / 1s

speed=TURN_MIN; / / povoleni rizeni rychlosti zataceni pres preruseni enable_interrupts(INT_TIMER2); enable_interrupts(GLOBAL); / / Cik-Cak ------------------------------------------------------------sensors=S; line=S; cik_cak(); / / toc se, abys nasel caru Delay_ms(500); Beep(1000,200); Delay_ms(500); dira=0; straight=255;

/ / inicializace globalnich promennych

while(true) {

/ / hlavni smycka (jizda podle cary)

sensors = RSENSOR; sensors |= LSENSOR << 1;

/ / cteni senzoru na caru

if (sensors != 0) line=sensors;

/ / kdyz vidis caru, / / poznamenej, kdes ji videl

if (input(BUMPER)) / / obsluha narazniku { unsigned int16 brzda; BL; BR; / / zpetny chod if (straight>rovinka) {brzda=8000;} else {brzda=4000;}; for (n=0; n<=brzda; n++) / / zabrzdi { sensors = RSENSOR; / / cteni senzoru na caru sensors |= LSENSOR << 1; / / pokud se pri brzdeni narazi na caru if(sensors!=0) line=sensors; / / poznamenej to }; STOPL; STOPR; for (n=500; n<800; n+=100) { beep(n,n); / /beep UP }; for (n=0; n<=4000; n++) / / vzad { GO(L, B, 200); GO(R, B, 200); }; STOPL; STOPR; beep(2000,300); for (n=0; n<=5000; n++) / / vpravo { GO(L, F, 200); GO(R, B, 200); }; STOPL; STOPR; beep(2000,300); for (n=0; n<=12000; n++) / / rovne { GO(L, F, 250); GO(R, F, 250); }; STOPL; STOPR;


25-IV-05 Page 12:45 6 of am8

beep(2000,300); for (n=0; n<=7000; n++) / / vlevo { GO(L, B, 200); GO(R, F, 200); }; STOPL; STOPR; beep(2000,300); for (n=0; n<=15000; n++) / / rovne { GO(L, F, 250); GO(R, F, 250); }; for (n=0; n<=20000; n++) / / rovne { GO(L, F, 250); GO(R, F, 250); if (LSENSOR || RSENSOR) break; }; BL; BR; beep(2000,300); STOPL; STOPR; for (n=0; n<=40000; n++) / / rovne { GO(L, F, TURN_MIN); if (LSENSOR || RSENSOR) break; }; STOPL; STOPR; beep(2000,300); line=L; speed=TURN_MIN; cik_cak(); }; #IFDEF DEBUG / / ladici vypis v debug verzi sprintf(AXstring,"Line: %U Movement: %U\0", line, movement); SendPacket(&AXstring[0]); / / Posli telemetrii delay_ms(1000); #ENDIF

/ / DATA to String.

switch (sensors) / / zatacej podle toho, kde vidis caru { case S: / / rovne 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; case L: / / trochu vlevo 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+((TURN_MIN-TURN_MAX)>>1); dira=0; continue; case R: / / trochu vpravo 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+((TURN_MIN-TURN_MAX)>>1); dira=0; continue;

C:\dokumenty\@Dokumenty\Roboti\IstRobot2005\FW\Tank\tank.c default:

25-IV-05 Page 12:45 7 of am8

/ / kdyz jsou obe cidla mimo caru, tak pokracuj dal

}

if (straight>ROVINKA) { int pom; unsigned int16 n;

/ / pokud byla dlouha rovinka, tak zabrzdi

if (L==line) / / brzdi jenom jednim pasem, zatoc setrvacnosti { STOPR; BL; } else { STOPL; BR; }; for (n=0; n<=BRZDENI; n++) / / prodleva { sensors = RSENSOR; / / cteni senzoru na caru sensors |= LSENSOR << 1; / / pokud se pri brzdeni narazi na caru if(sensors!=0) line=sensors; / / poznamenej to }; STOPL; STOPR; dira=0; }; straight=0;

/ / nuluj pocitadlo rovinky

if (L==line) / / 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); for(n=BW_PO_DIRE ;n>0 ; n--) { if (L==line) / / zpet, podle toho kam se jelo { GO(R, B, speed); } else { GO(L, B, speed); }; sensors = RSENSOR; / / cteni senzoru na caru sensors |= LSENSOR << 1; / / pokud se pri brzdeni narazi na caru if(sensors!=0) { line=sensors; / / poznamenej to STOPL; STOPR; break; }; };


25-IV-05 Page 12:45 8 of am8

if (L==line) / / zpet, podle toho kam se jelo { line=R; / / cara je na druhe strane } else { line=L; / / cara je na druhe strane }; STOPL; STOPR; for(n=BW_PO_DIRE ;n>0 ; n--) { sensors = RSENSOR; / / cteni senzoru na caru sensors |= LSENSOR << 1; / / pokud se pri brzdeni narazi na caru if(sensors!=0) { line=sensors; / / poznamenej to break; }; }; cik_cak(); / / najdi caru }; } / / while(true) }

#define DEBUG

Recommend Documents