/roboti/istrobot/merkur/PIC16F88/turn_R/AX25.c |
---|
0,0 → 1,135 |
#nolist |
//#define PTT PIN_A2 // PTT control |
//#define TXo PIN_C0 // To the transmitter modulator |
#define PERIODAH delay_us(222) // Halfperiod H 222;78/1200 500;430/500 |
#define TAILH delay_us(78) |
#define PERIODAL delay_us(412) // Halfperiod L 412;345/1200 1000;880/500 |
#define TAILL delay_us(345) |
#byte STATUS = 3 // CPUs status register |
byte SendData[16] = {'A'<<1, 'L'<<1, 'L'<<1, ' '<<1, ' '<<1, ' '<<1, 0x60, |
'C'<<1, 'Z'<<1, '0'<<1, 'R'<<1, 'R'<<1, 'R'<<1, 0x61, |
0x03, 0xF0}; |
boolean bit; |
int fcslo, fcshi; // variabloes for calculating FCS (CRC) |
int stuff; // stuff counter for extra 0 |
int flag_flag; // if it is sending flag (7E) |
int fcs_flag; // if it is sending Frame Check Sequence |
int i; // for for |
void flipout() //flips the state of output pin a_1 |
{ |
stuff = 0; //since this is a 0, reset the stuff counter |
if (bit) |
{ |
bit=FALSE; //if the state of the pin was low, make it high. |
} |
else |
{ |
bit=TRUE; //if the state of the pin was high make it low |
} |
} |
void fcsbit(byte tbyte) |
{ |
#asm |
BCF STATUS,0 |
RRF fcshi,F // rotates the entire 16 bits |
RRF fcslo,F // to the right |
#endasm |
if (((STATUS & 0x01)^(tbyte)) ==0x01) |
{ |
fcshi = fcshi^0x84; |
fcslo = fcslo^0x08; |
} |
} |
void SendBit () |
{ |
if (bit) |
{ |
output_high(TXo); |
PERIODAH; |
output_low(TXo); |
PERIODAH; |
output_high(TXo); |
PERIODAH; |
output_low(TXo); |
TAILH; |
} |
else |
{ |
output_high(TXo); |
PERIODAL; |
output_low(TXo); |
TAILL; |
}; |
} |
void SendByte (byte inbyte) |
{ |
int k, bt; |
for (k=0;k<8;k++) //do the following for each of the 8 bits in the byte |
{ |
bt = inbyte & 0x01; //strip off the rightmost bit of the byte to be sent (inbyte) |
if ((fcs_flag == FALSE) & (flag_flag == FALSE)) fcsbit(bt); //do FCS calc, but only if this |
//is not a flag or fcs byte |
if (bt == 0) |
{ |
flipout(); |
} // if this bit is a zero, flip the output state |
else |
{ //otherwise if it is a 1, do the following: |
if (flag_flag == FALSE) stuff++; //increment the count of consequtive 1's |
if ((flag_flag == FALSE) & (stuff == 5)) |
{ //stuff an extra 0, if 5 1's in a row |
SendBit(); |
flipout(); //flip the output state to stuff a 0 |
}//end of if |
}//end of else |
// delay_us(850); //introduces a delay that creates 1200 baud |
SendBit(); |
inbyte = inbyte>>1; //go to the next bit in the byte |
}//end of for |
}//end of SendByte |
void SendPacket(char *data) |
{ |
bit=FALSE; |
fcslo=fcshi=0xFF; //The 2 FCS Bytes are initialized to FF |
stuff = 0; //The variable stuff counts the number of 1's in a row. When it gets to 5 |
// it is time to stuff a 0. |
// output_low(PTT); // Blinking LED |
// delay_ms(1000); |
// output_high(PTT); |
flag_flag = TRUE; //The variable flag is true if you are transmitted flags (7E's) false otherwise. |
fcs_flag = FALSE; //The variable fcsflag is true if you are transmitting FCS bytes, false otherwise. |
for(i=0; i<10; i++) SendByte(0x7E); //Sends flag bytes. Adjust length for txdelay |
//each flag takes approx 6.7 ms |
flag_flag = FALSE; //done sending flags |
for(i=0; i<16; i++) SendByte(SendData[i]); //send the packet bytes |
for(i=0; 0 != *data; i++) |
{ |
SendByte(*data); //send the packet bytes |
data++; |
}; |
fcs_flag = TRUE; //about to send the FCS bytes |
fcslo =fcslo^0xff; //must XOR them with FF before sending |
fcshi = fcshi^0xff; |
SendByte(fcslo); //send the low byte of fcs |
SendByte(fcshi); //send the high byte of fcs |
fcs_flag = FALSE; //done sending FCS |
flag_flag = TRUE; //about to send flags |
SendByte(0x7e); // Send a flag to end packet |
} |
#list |
/roboti/istrobot/merkur/PIC16F88/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) |
} |
/roboti/istrobot/merkur/PIC16F88/turn_R/tank.HEX |
---|
0,0 → 1,202 |
:1000000000308A00722C0000FF00030E8301A10063 |
:100010007F08A0000A08A8008A01A00E0408A20018 |
:100020007708A3007808A4007908A5007A08A6003C |
:100030007B08A700831383128C308400801C222845 |
:100040008C183528220884002308F7002408F800BB |
:100050002508F9002608FA002708FB0028088A006E |
:10006000210E8300FF0E7F0E09008A113728420FF0 |
:100070003A283C280130C20743080E3C0318C30A43 |
:100080004608783C0318C60A8C108A1122281230C0 |
:100090008316A202031C5A28A2308400FC3080057B |
:1000A0000310800C800C000803195A2858280000FF |
:1000B000800B5728831200347D088316A1007C082A |
:1000C000A000A008031D6728A108031986280513AE |
:1000D00083120517831685138312851374088316FC |
:1000E000A200831247208316851383128517831677 |
:1000F00005138312051374088316A2008312472088 |
:10010000831620080319A103A00361288312003479 |
:1001100074308400000803199C280130F800F701AE |
:10012000F70B9028F80B8F284A30F700F70B96282A |
:1001300000000000800B8D28003483168615831282 |
:10014000861DC8290130F200F430F1007208033C2A |
:10015000031CBE28031DB02871081F3C031CBE28C9 |
:100160007208F5007108F4007208FD007108FC00C7 |
:100170005C206430F1070318F20AA6280430F3006B |
:10018000FA30F4008820F30BC0288316061383127C |
:1001900006138316861383128613831606128312A0 |
:1001A0000612831686128312861283168612831213 |
:1001B000861283160612831206160430F300FA30F4 |
:1001C000F4008820F30BDF28831606128312061230 |
:1001D00083168612831286120430F300FA30F4007C |
:1001E0008820F30BEE28831606128312061283165C |
:1001F0008612831286160430F300FA30F400882049 |
:10020000F30BFD288316061283120612831686123C |
:10021000831286120430F300FA30F4008820F30BC6 |
:100220000C290330F5007030F400FD016430FC004F |
:100230005C200430F300FA30F4008820F30B1B2913 |
:1002400083168613831286138316061383120617EA |
:100250000430F300FA30F4008820F30B2A298316C7 |
:100260000613831206138316861383128613043033 |
:10027000F300FA30F4008820F30B392983160613B3 |
:100280008312061383168613831286170430F30035 |
:10029000FA30F4008820F30B4829831606138312E2 |
:1002A000061383168613831286130430F300FA3084 |
:1002B000F4008820F30B57290330F5007030F40068 |
:1002C000FD016430FC005C200430F300FA30F400DF |
:1002D0008820F30B662983168613831286138316F0 |
:1002E000061383120617831686128312861283164C |
:1002F0000612831206160430F300FA30F400882048 |
:10030000F30B7D29831606138312061383168613B7 |
:100310008312861383160612831206128316861220 |
:10032000831286120430F300FA30F4008820F30BB5 |
:1003300094298316061383120613831686138312D9 |
:1003400086178316061283120612831686128312EC |
:1003500086160430F300FA30F4008820F30BAB2942 |
:1003600083160613831206138316861383128613CD |
:1003700083160612831206128316861283128612C1 |
:100380000430F300FA30F4008820F30BC2299D28D2 |
:10039000831606158312061DFB2983169C1FDB2975 |
:1003A0000330F5008430F4000130FD00F430FC002F |
:1003B00083125C2083161C1FE8290330F5002030CF |
:1003C000F4000130FD00F430FC0083125C20831641 |
:1003D00083121F1DED298316E8291E087F3C031C8C |
:1003E000FA290330F500E830F4000130FD00F43064 |
:1003F000FC005C20C8298A11202D7530F5003030B2 |
:10040000F400003083169C1B0130003A03190A2ABD |
:100410001C1F642A7408303C031D5E2A7508753C55 |
:10042000031D5E2AF501F40183124508023A0319FF |
:10043000202A033A0319332A023A0319462A5D2A6D |
:1004400083168613831286138316061383120617E8 |
:1004500083160612831206128316861283128616DC |
:100460000130C5005D2A8316861283128612831618 |
:1004700006128312061683160613831206138316BA |
:100480008613831286170230C5005D2A83168613F1 |
:100490008312861383160613831206178316061219 |
:1004A0008312061283168612831286160130C50047 |
:1004B0003A30F5009830F4005D2A8316F40A0319E7 |
:1004C000F50A8312012A8316061383120613831674 |
:1004D000861383128613831606128312061283165E |
:1004E000861283128612C10183169C1F7A2A8312F8 |
:1004F000C10A831600301C1B0130F7000310F70DF2 |
:1005000077088312C104C1080319FD290330C10013 |
:100510000034831606138312061383168613831280 |
:10052000861783160612831206128316861283120A |
:1005300086169630F400882083160612831206125F |
:1005400083168612831286128316061383120613ED |
:1005500083168613831286130330F5008430F4006B |
:100560000330FD00E830FC005C2083160612831285 |
:10057000061283168612831286168316861383123A |
:10058000861383160613831206170230F300873092 |
:10059000F4008820F30BC72A831686128312861272 |
:1005A0008316061283120616831686138312861389 |
:1005B00083160613831206170230F300FA30F40094 |
:1005C0008820F30BDE2A8316061383120613831684 |
:1005D0008613831286171E30F400882083160613B4 |
:1005E00083120613831686138312861383168612CC |
:1005F0008312861283160612831206160230F30047 |
:10060000FA30F4008820F30B002B83168612831235 |
:100610008612831606128312061683168613831219 |
:10062000861383160613831206176430F40088209D |
:100630009C30F2004030F100F108031D222BF2083B |
:1006400003193E2BC10183169C1F292B8312C10A5B |
:10065000831600301C1B0130F7000310F70D7708DC |
:100660008312C104C1080319392B9630F400882085 |
:100670003E2B71080319F203F1031C2B831606129B |
:100680008312061283168612831286168316861329 |
:100690008312861383160613831206179C30F2000A |
:1006A0004030F100F108031D582BF2080319702B9C |
:1006B000C10183169C1F5F2B8312C10A8316003071 |
:1006C0001C1B0130F7000310F70D77088312C104DB |
:1006D000C108031D702B71080319F203F103522B9B |
:1006E000831606128312061283168612831286124E |
:1006F000831606138312061383168613831286133A |
:100700000230C500FD21C6018A11502D8316061343 |
:10071000831206138316861383128613831606121A |
:100720008312061283168612831286124208F30081 |
:100730000330F500E830F4000130FD00F430FC0037 |
:100740005C204508023A0319AC2B033A0319E42B49 |
:10075000023A03191C2C1E2C0230F200EE30F1007C |
:10076000F108031DB62BF2080319D32B01087302FD |
:10077000031CC32B831606128312061283168612DD |
:1007800083128616CB2B831606128312061283164B |
:100790008612831286120130F40088207108031932 |
:1007A000F203F103B02B831606138312061383168C |
:1007B000861383128613831606128312061283167B |
:1007C0008612831286121E2C0230F200EE30F100E7 |
:1007D000F108031DEE2BF20803190B2C010873021C |
:1007E000031CFB2B83160613831206138316861332 |
:1007F00083128617032C831606138312061383169F |
:100800008613831286130130F400882071080319BF |
:10081000F203F103E82B83160613831206138316E3 |
:10082000861383128613831606128312061283160A |
:100830008612831286121E2C5C2C1E2C0330F500AF |
:10084000E830F4000130FD00F430FC005C20831639 |
:1008500086128312861283160612831206168316D8 |
:10086000861383128613831606138312061701302C |
:10087000F2002C30F100F108031D412CF20803199D |
:100880005C2CC10183169C1F482C8312C10A83165D |
:1008900000301C1B0130F7000310F70D770883129E |
:1008A000C104C108031D5C2C0130F40088207108CC |
:1008B0000319F203F1033B2C83160613831206136C |
:1008C000831686138312861383160612831206126A |
:1008D00083168612831286120330C500FD21C601DD |
:1008E0008A11E62D84011F30830583161F129F1283 |
:1008F0001B0880399B0007309C001C0883120D13D5 |
:10090000603083168F0082308312A9009830AA00CD |
:10091000AB004030AC00AD00AE006030AF008630C0 |
:10092000B000B430B1006030B200A430B300B40005 |
:10093000B5006130B6000330B700F030B800831660 |
:1009400006138312061383168613831286138316E7 |
:1009500006128312061283168612831286126230E2 |
:1009600083168F0081138312941283160611861446 |
:100970000612003083129400831694000108C739D0 |
:10098000083881004830F800053883129200FF30A3 |
:10099000831692001F129F121B08803904389B0097 |
:1009A0001F1383121F179F1783169F1383121F1481 |
:1009B0001030F8001F08C73978049F008530900078 |
:1009C000831686150B308312970096010730950029 |
:1009D000023083169C000508033885000330F700B9 |
:1009E000F70BF02C1C0883120D13863083169D0024 |
:1009F0000330F500E830F400FD01C830FC0083123C |
:100A00005C203230F40088200330F500E830F40038 |
:100A1000FD01C830FC005C200430F100FA30F40025 |
:100A20008820F10B0E2D83168C14C03083128B049A |
:100A30000330C000C100C400C500E630C2009D28DC |
:100A40000230F100FA30F4008820F10B222D03303F |
:100A5000F500E830F400FD01C830FC005C200230F5 |
:100A6000F100FA30F4008820F10B312DC00183161B |
:100A70009C1F3D2D8312C00A831600301C1B0130C1 |
:100A8000F7000310F70D77088312C0041F19462DD5 |
:100A90001E087F3C031C502D4608323C0318892A4F |
:100AA0004008033A03195B2D013A03196F2D033AED |
:100AB0000319A82DE12D831686138312861383163E |
:100AC0000613831206178316861283128612831664 |
:100AD000061283120616C6010330C500362D010822 |
:100AE000F100E630430771020319782D0318812DB8 |
:100AF0008316861383128613831606138312061732 |
:100B0000892D831606138312061383168613831208 |
:100B100086130108F100F030430771020319922D8A |
:100B200003189B2D831686128312861283160612D3 |
:100B300083120616A32D831606128312061283163D |
:100B40008612831286120230C100C601C500362DFE |
:100B50000108F100E630430771020319B12D0318B3 |
:100B6000BA2D8316861283128612831606128312FA |
:100B70000616C22D831606128312061283168612DB |
:100B8000831286120108F100F03043077102031945 |
:100B9000CB2D0318D42D8316861383128613831648 |
:100BA000061383120617DC2D831606138312061311 |
:100BB00083168613831286130130C100C601C50057 |
:100BC000362DC3014608773C031C862B410844029E |
:100BD0000319EE2D4108C400F030C2004108023C68 |
:100BE000031D122E83160613831206138316861313 |
:100BF0008312861301084202031C072E83168612F5 |
:100C00008312861283160612831206160F2E83167F |
:100C1000061283120612831686128312861202307F |
:100C2000C500312E83160612831206128316861211 |
:100C30008312861201084202031C272E8316861394 |
:100C40008312861383160613831206172F2E83161C |
:100C5000061383120613831686138312861301303C |
:060C6000C500362D630003 |
:04400E00383FFC3FFC |
:00000001FF |
;PIC16F88 |
/roboti/istrobot/merkur/PIC16F88/turn_R/tank.LST |
---|
0,0 → 1,1932 |
CCS PCM C Compiler, Version 3.221, 27853 27-IV-05 14:16 |
Filename: D:\KAKLIK\programy\PIC_C\roboti\merkur\PIC16F88\turn_R\tank.LST |
ROM used: 1587 words (39%) |
Largest free fragment is 2048 |
RAM used: 87 (50%) at main() level |
98 (56%) worst case |
Stack: 4 worst case (3 in main + 1 for interrupts) |
* |
0000: MOVLW 00 |
0001: MOVWF 0A |
0002: GOTO 472 |
0003: NOP |
0004: MOVWF 7F |
0005: SWAPF 03,W |
0006: CLRF 03 |
0007: MOVWF 21 |
0008: MOVF 7F,W |
0009: MOVWF 20 |
000A: MOVF 0A,W |
000B: MOVWF 28 |
000C: CLRF 0A |
000D: SWAPF 20,F |
000E: MOVF 04,W |
000F: MOVWF 22 |
0010: MOVF 77,W |
0011: MOVWF 23 |
0012: MOVF 78,W |
0013: MOVWF 24 |
0014: MOVF 79,W |
0015: MOVWF 25 |
0016: MOVF 7A,W |
0017: MOVWF 26 |
0018: MOVF 7B,W |
0019: MOVWF 27 |
001A: BCF 03.7 |
001B: BCF 03.5 |
001C: MOVLW 8C |
001D: MOVWF 04 |
001E: BTFSS 00.1 |
001F: GOTO 022 |
0020: BTFSC 0C.1 |
0021: GOTO 035 |
0022: MOVF 22,W |
0023: MOVWF 04 |
0024: MOVF 23,W |
0025: MOVWF 77 |
0026: MOVF 24,W |
0027: MOVWF 78 |
0028: MOVF 25,W |
0029: MOVWF 79 |
002A: MOVF 26,W |
002B: MOVWF 7A |
002C: MOVF 27,W |
002D: MOVWF 7B |
002E: MOVF 28,W |
002F: MOVWF 0A |
0030: SWAPF 21,W |
0031: MOVWF 03 |
0032: SWAPF 7F,F |
0033: SWAPF 7F,W |
0034: RETFIE |
0035: BCF 0A.3 |
0036: GOTO 037 |
.................... #include "tank.h" |
.................... #include <16F88.h> |
.................... //////// Standard Header file for the PIC16F88 device //////////////// |
.................... #device PIC16F88 |
.................... #list |
.................... |
.................... #device adc=8 |
.................... #fuses NOWDT,INTRC_IO, NOPUT, MCLR, NOBROWNOUT, NOLVP, NOCPD, NOWRT, NODEBUG, NOPROTECT, NOFCMEN, NOIESO |
.................... #use delay(clock=4000000) |
* |
0047: MOVLW 12 |
0048: BSF 03.5 |
0049: SUBWF 22,F |
004A: BTFSS 03.0 |
004B: GOTO 05A |
004C: MOVLW A2 |
004D: MOVWF 04 |
004E: MOVLW FC |
004F: ANDWF 00,F |
0050: BCF 03.0 |
0051: RRF 00,F |
0052: RRF 00,F |
0053: MOVF 00,W |
0054: BTFSC 03.2 |
0055: GOTO 05A |
0056: GOTO 058 |
0057: NOP |
0058: DECFSZ 00,F |
0059: GOTO 057 |
005A: BCF 03.5 |
005B: RETLW 00 |
* |
0088: MOVLW 74 |
0089: MOVWF 04 |
008A: MOVF 00,W |
008B: BTFSC 03.2 |
008C: GOTO 09C |
008D: MOVLW 01 |
008E: MOVWF 78 |
008F: CLRF 77 |
0090: DECFSZ 77,F |
0091: GOTO 090 |
0092: DECFSZ 78,F |
0093: GOTO 08F |
0094: MOVLW 4A |
0095: MOVWF 77 |
0096: DECFSZ 77,F |
0097: GOTO 096 |
0098: NOP |
0099: NOP |
009A: DECFSZ 00,F |
009B: GOTO 08D |
009C: RETLW 00 |
.................... |
.................... |
.................... |
.................... #define DEBUG |
.................... |
.................... #define TXo PIN_A3 // To the transmitter modulator |
.................... #include "AX25.c" // podprogram pro prenos telemetrie |
.................... #list |
.................... |
.................... |
.................... 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; |
* |
0037: INCFSZ 42,W |
0038: GOTO 03A |
0039: GOTO 03C |
003A: MOVLW 01 |
003B: ADDWF 42,F |
.................... if (rovinka<MAX_ROVINKA) rovinka++; |
003C: MOVF 43,W |
003D: SUBLW 0E |
003E: BTFSC 03.0 |
003F: INCF 43,F |
.................... if (dira<=T_DIRA) dira++; |
0040: MOVF 46,W |
0041: SUBLW 78 |
0042: BTFSC 03.0 |
0043: INCF 46,F |
.................... } |
.................... // Primitivni Pipani |
0044: BCF 0C.1 |
0045: BCF 0A.3 |
0046: GOTO 022 |
.................... void beep(unsigned int16 period, unsigned int16 length) |
.................... { |
.................... unsigned int16 nn; |
.................... |
.................... for(nn=length; nn>0; nn--) |
* |
005C: MOVF 7D,W |
005D: BSF 03.5 |
005E: MOVWF 21 |
005F: MOVF 7C,W |
0060: MOVWF 20 |
0061: MOVF 20,F |
0062: BTFSS 03.2 |
0063: GOTO 067 |
0064: MOVF 21,F |
0065: BTFSC 03.2 |
0066: GOTO 086 |
.................... { |
.................... output_high(SOUND_HI);output_low(SOUND_LO); |
0067: BCF 05.6 |
0068: BCF 03.5 |
0069: BSF 05.6 |
006A: BSF 03.5 |
006B: BCF 05.7 |
006C: BCF 03.5 |
006D: BCF 05.7 |
.................... delay_us(period); |
006E: MOVF 74,W |
006F: BSF 03.5 |
0070: MOVWF 22 |
0071: BCF 03.5 |
0072: CALL 047 |
.................... output_high(SOUND_LO);output_low(SOUND_HI); |
0073: BSF 03.5 |
0074: BCF 05.7 |
0075: BCF 03.5 |
0076: BSF 05.7 |
0077: BSF 03.5 |
0078: BCF 05.6 |
0079: BCF 03.5 |
007A: BCF 05.6 |
.................... delay_us(period); |
007B: MOVF 74,W |
007C: BSF 03.5 |
007D: MOVWF 22 |
007E: BCF 03.5 |
007F: CALL 047 |
.................... } |
0080: BSF 03.5 |
0081: MOVF 20,W |
0082: BTFSC 03.2 |
0083: DECF 21,F |
0084: DECF 20,F |
0085: GOTO 061 |
.................... } |
0086: BCF 03.5 |
0087: RETLW 00 |
.................... /******************************************************************************/ |
.................... void diagnostika() |
.................... { |
.................... unsigned int16 n; |
.................... |
.................... while (input(DIAG_SERVO)) // Propojka, ktera spousti diagnostiku |
.................... { |
* |
009D: BSF 03.5 |
009E: BSF 06.3 |
009F: BCF 03.5 |
00A0: BTFSS 06.3 |
00A1: GOTO 1C8 |
.................... for (n=500; n<800; n+=100) |
00A2: MOVLW 01 |
00A3: MOVWF 72 |
00A4: MOVLW F4 |
00A5: MOVWF 71 |
00A6: MOVF 72,W |
00A7: SUBLW 03 |
00A8: BTFSS 03.0 |
00A9: GOTO 0BE |
00AA: BTFSS 03.2 |
00AB: GOTO 0B0 |
00AC: MOVF 71,W |
00AD: SUBLW 1F |
00AE: BTFSS 03.0 |
00AF: GOTO 0BE |
.................... { |
.................... beep(n,n); //beep UP |
00B0: MOVF 72,W |
00B1: MOVWF 75 |
00B2: MOVF 71,W |
00B3: MOVWF 74 |
00B4: MOVF 72,W |
00B5: MOVWF 7D |
00B6: MOVF 71,W |
00B7: MOVWF 7C |
00B8: CALL 05C |
.................... }; |
00B9: MOVLW 64 |
00BA: ADDWF 71,F |
00BB: BTFSC 03.0 |
00BC: INCF 72,F |
00BD: GOTO 0A6 |
.................... Delay_ms(1000); |
00BE: MOVLW 04 |
00BF: MOVWF 73 |
00C0: MOVLW FA |
00C1: MOVWF 74 |
00C2: CALL 088 |
00C3: DECFSZ 73,F |
00C4: GOTO 0C0 |
.................... //zastav vse |
.................... STOPL; STOPR; |
00C5: BSF 03.5 |
00C6: BCF 06.6 |
00C7: BCF 03.5 |
00C8: BCF 06.6 |
00C9: BSF 03.5 |
00CA: BCF 06.7 |
00CB: BCF 03.5 |
00CC: BCF 06.7 |
00CD: BSF 03.5 |
00CE: BCF 06.4 |
00CF: BCF 03.5 |
00D0: BCF 06.4 |
00D1: BSF 03.5 |
00D2: BCF 06.5 |
00D3: BCF 03.5 |
00D4: BCF 06.5 |
.................... //pravy pas |
.................... FR; Delay_ms(1000); STOPR; Delay_ms(1000); |
00D5: BSF 03.5 |
00D6: BCF 06.5 |
00D7: BCF 03.5 |
00D8: BCF 06.5 |
00D9: BSF 03.5 |
00DA: BCF 06.4 |
00DB: BCF 03.5 |
00DC: BSF 06.4 |
00DD: MOVLW 04 |
00DE: MOVWF 73 |
00DF: MOVLW FA |
00E0: MOVWF 74 |
00E1: CALL 088 |
00E2: DECFSZ 73,F |
00E3: GOTO 0DF |
00E4: BSF 03.5 |
00E5: BCF 06.4 |
00E6: BCF 03.5 |
00E7: BCF 06.4 |
00E8: BSF 03.5 |
00E9: BCF 06.5 |
00EA: BCF 03.5 |
00EB: BCF 06.5 |
00EC: MOVLW 04 |
00ED: MOVWF 73 |
00EE: MOVLW FA |
00EF: MOVWF 74 |
00F0: CALL 088 |
00F1: DECFSZ 73,F |
00F2: GOTO 0EE |
.................... BR; Delay_ms(1000); STOPR; Delay_ms(1000); |
00F3: BSF 03.5 |
00F4: BCF 06.4 |
00F5: BCF 03.5 |
00F6: BCF 06.4 |
00F7: BSF 03.5 |
00F8: BCF 06.5 |
00F9: BCF 03.5 |
00FA: BSF 06.5 |
00FB: MOVLW 04 |
00FC: MOVWF 73 |
00FD: MOVLW FA |
00FE: MOVWF 74 |
00FF: CALL 088 |
0100: DECFSZ 73,F |
0101: GOTO 0FD |
0102: BSF 03.5 |
0103: BCF 06.4 |
0104: BCF 03.5 |
0105: BCF 06.4 |
0106: BSF 03.5 |
0107: BCF 06.5 |
0108: BCF 03.5 |
0109: BCF 06.5 |
010A: MOVLW 04 |
010B: MOVWF 73 |
010C: MOVLW FA |
010D: MOVWF 74 |
010E: CALL 088 |
010F: DECFSZ 73,F |
0110: GOTO 10C |
.................... Beep(880,100); Delay_ms(1000); |
0111: MOVLW 03 |
0112: MOVWF 75 |
0113: MOVLW 70 |
0114: MOVWF 74 |
0115: CLRF 7D |
0116: MOVLW 64 |
0117: MOVWF 7C |
0118: CALL 05C |
0119: MOVLW 04 |
011A: MOVWF 73 |
011B: MOVLW FA |
011C: MOVWF 74 |
011D: CALL 088 |
011E: DECFSZ 73,F |
011F: GOTO 11B |
.................... //levy pas |
.................... FL; Delay_ms(1000); STOPL; Delay_ms(1000); |
0120: BSF 03.5 |
0121: BCF 06.7 |
0122: BCF 03.5 |
0123: BCF 06.7 |
0124: BSF 03.5 |
0125: BCF 06.6 |
0126: BCF 03.5 |
0127: BSF 06.6 |
0128: MOVLW 04 |
0129: MOVWF 73 |
012A: MOVLW FA |
012B: MOVWF 74 |
012C: CALL 088 |
012D: DECFSZ 73,F |
012E: GOTO 12A |
012F: BSF 03.5 |
0130: BCF 06.6 |
0131: BCF 03.5 |
0132: BCF 06.6 |
0133: BSF 03.5 |
0134: BCF 06.7 |
0135: BCF 03.5 |
0136: BCF 06.7 |
0137: MOVLW 04 |
0138: MOVWF 73 |
0139: MOVLW FA |
013A: MOVWF 74 |
013B: CALL 088 |
013C: DECFSZ 73,F |
013D: GOTO 139 |
.................... BL; Delay_ms(1000); STOPL; Delay_ms(1000); |
013E: BSF 03.5 |
013F: BCF 06.6 |
0140: BCF 03.5 |
0141: BCF 06.6 |
0142: BSF 03.5 |
0143: BCF 06.7 |
0144: BCF 03.5 |
0145: BSF 06.7 |
0146: MOVLW 04 |
0147: MOVWF 73 |
0148: MOVLW FA |
0149: MOVWF 74 |
014A: CALL 088 |
014B: DECFSZ 73,F |
014C: GOTO 148 |
014D: BSF 03.5 |
014E: BCF 06.6 |
014F: BCF 03.5 |
0150: BCF 06.6 |
0151: BSF 03.5 |
0152: BCF 06.7 |
0153: BCF 03.5 |
0154: BCF 06.7 |
0155: MOVLW 04 |
0156: MOVWF 73 |
0157: MOVLW FA |
0158: MOVWF 74 |
0159: CALL 088 |
015A: DECFSZ 73,F |
015B: GOTO 157 |
.................... Beep(880,100); Delay_ms(1000); |
015C: MOVLW 03 |
015D: MOVWF 75 |
015E: MOVLW 70 |
015F: MOVWF 74 |
0160: CLRF 7D |
0161: MOVLW 64 |
0162: MOVWF 7C |
0163: CALL 05C |
0164: MOVLW 04 |
0165: MOVWF 73 |
0166: MOVLW FA |
0167: MOVWF 74 |
0168: CALL 088 |
0169: DECFSZ 73,F |
016A: GOTO 166 |
.................... //oba pasy |
.................... FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000); |
016B: BSF 03.5 |
016C: BCF 06.7 |
016D: BCF 03.5 |
016E: BCF 06.7 |
016F: BSF 03.5 |
0170: BCF 06.6 |
0171: BCF 03.5 |
0172: BSF 06.6 |
0173: BSF 03.5 |
0174: BCF 06.5 |
0175: BCF 03.5 |
0176: BCF 06.5 |
0177: BSF 03.5 |
0178: BCF 06.4 |
0179: BCF 03.5 |
017A: BSF 06.4 |
017B: MOVLW 04 |
017C: MOVWF 73 |
017D: MOVLW FA |
017E: MOVWF 74 |
017F: CALL 088 |
0180: DECFSZ 73,F |
0181: GOTO 17D |
0182: BSF 03.5 |
0183: BCF 06.6 |
0184: BCF 03.5 |
0185: BCF 06.6 |
0186: BSF 03.5 |
0187: BCF 06.7 |
0188: BCF 03.5 |
0189: BCF 06.7 |
018A: BSF 03.5 |
018B: BCF 06.4 |
018C: BCF 03.5 |
018D: BCF 06.4 |
018E: BSF 03.5 |
018F: BCF 06.5 |
0190: BCF 03.5 |
0191: BCF 06.5 |
0192: MOVLW 04 |
0193: MOVWF 73 |
0194: MOVLW FA |
0195: MOVWF 74 |
0196: CALL 088 |
0197: DECFSZ 73,F |
0198: GOTO 194 |
.................... BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000); |
0199: BSF 03.5 |
019A: BCF 06.6 |
019B: BCF 03.5 |
019C: BCF 06.6 |
019D: BSF 03.5 |
019E: BCF 06.7 |
019F: BCF 03.5 |
01A0: BSF 06.7 |
01A1: BSF 03.5 |
01A2: BCF 06.4 |
01A3: BCF 03.5 |
01A4: BCF 06.4 |
01A5: BSF 03.5 |
01A6: BCF 06.5 |
01A7: BCF 03.5 |
01A8: BSF 06.5 |
01A9: MOVLW 04 |
01AA: MOVWF 73 |
01AB: MOVLW FA |
01AC: MOVWF 74 |
01AD: CALL 088 |
01AE: DECFSZ 73,F |
01AF: GOTO 1AB |
01B0: BSF 03.5 |
01B1: BCF 06.6 |
01B2: BCF 03.5 |
01B3: BCF 06.6 |
01B4: BSF 03.5 |
01B5: BCF 06.7 |
01B6: BCF 03.5 |
01B7: BCF 06.7 |
01B8: BSF 03.5 |
01B9: BCF 06.4 |
01BA: BCF 03.5 |
01BB: BCF 06.4 |
01BC: BSF 03.5 |
01BD: BCF 06.5 |
01BE: BCF 03.5 |
01BF: BCF 06.5 |
01C0: MOVLW 04 |
01C1: MOVWF 73 |
01C2: MOVLW FA |
01C3: MOVWF 74 |
01C4: CALL 088 |
01C5: DECFSZ 73,F |
01C6: GOTO 1C2 |
.................... }; |
01C7: GOTO 09D |
.................... while (input(DIAG_SENSORS)) // spusteni diagnostiky cidel |
.................... { |
01C8: BSF 03.5 |
01C9: BSF 06.2 |
01CA: BCF 03.5 |
01CB: BTFSS 06.2 |
01CC: GOTO 1FB |
.................... if (RSENSOR) beep(900,500); |
01CD: BSF 03.5 |
01CE: BTFSS 1C.7 |
01CF: GOTO 1DB |
01D0: MOVLW 03 |
01D1: MOVWF 75 |
01D2: MOVLW 84 |
01D3: MOVWF 74 |
01D4: MOVLW 01 |
01D5: MOVWF 7D |
01D6: MOVLW F4 |
01D7: MOVWF 7C |
01D8: BCF 03.5 |
01D9: CALL 05C |
01DA: BSF 03.5 |
.................... if (LSENSOR) beep(800,500); |
01DB: BTFSS 1C.6 |
01DC: GOTO 1E8 |
01DD: MOVLW 03 |
01DE: MOVWF 75 |
01DF: MOVLW 20 |
01E0: MOVWF 74 |
01E1: MOVLW 01 |
01E2: MOVWF 7D |
01E3: MOVLW F4 |
01E4: MOVWF 7C |
01E5: BCF 03.5 |
01E6: CALL 05C |
01E7: BSF 03.5 |
.................... if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD)) beep(1000,500); |
01E8: BCF 03.5 |
01E9: BTFSS 1F.2 |
01EA: GOTO 1ED |
01EB: BSF 03.5 |
01EC: GOTO 1E8 |
01ED: MOVF 1E,W |
01EE: SUBLW 7F |
01EF: BTFSS 03.0 |
01F0: GOTO 1FA |
01F1: MOVLW 03 |
01F2: MOVWF 75 |
01F3: MOVLW E8 |
01F4: MOVWF 74 |
01F5: MOVLW 01 |
01F6: MOVWF 7D |
01F7: MOVLW F4 |
01F8: MOVWF 7C |
01F9: CALL 05C |
.................... }; |
01FA: GOTO 1C8 |
.................... } |
01FB: BCF 0A.3 |
01FC: GOTO 520 (RETURN) |
.................... /////////////////////////////////////////////////////////////////////////////// |
.................... void cikcak() |
.................... { |
.................... unsigned int16 n; |
.................... sem1: |
.................... n=CIK_CAK; |
01FD: MOVLW 75 |
01FE: MOVWF 75 |
01FF: MOVLW 30 |
0200: MOVWF 74 |
.................... while (0==RSENSOR||LSENSOR) // zkontroluj caru |
.................... { |
0201: MOVLW 00 |
0202: BSF 03.5 |
0203: BTFSC 1C.7 |
0204: MOVLW 01 |
0205: XORLW 00 |
0206: BTFSC 03.2 |
0207: GOTO 20A |
0208: BTFSS 1C.6 |
0209: GOTO 264 |
.................... if (n==CIK_CAK) // zmen smer zataceni |
020A: MOVF 74,W |
020B: SUBLW 30 |
020C: BTFSS 03.2 |
020D: GOTO 25E |
020E: MOVF 75,W |
020F: SUBLW 75 |
0210: BTFSS 03.2 |
0211: GOTO 25E |
.................... { |
.................... n=0; |
0212: CLRF 75 |
0213: CLRF 74 |
.................... switch(movement) |
.................... { |
0214: BCF 03.5 |
0215: MOVF 45,W |
0216: XORLW 02 |
0217: BTFSC 03.2 |
0218: GOTO 220 |
0219: XORLW 03 |
021A: BTFSC 03.2 |
021B: GOTO 233 |
021C: XORLW 02 |
021D: BTFSC 03.2 |
021E: GOTO 246 |
021F: GOTO 25D |
.................... case L: |
.................... FL;BR; |
0220: BSF 03.5 |
0221: BCF 06.7 |
0222: BCF 03.5 |
0223: BCF 06.7 |
0224: BSF 03.5 |
0225: BCF 06.6 |
0226: BCF 03.5 |
0227: BSF 06.6 |
0228: BSF 03.5 |
0229: BCF 06.4 |
022A: BCF 03.5 |
022B: BCF 06.4 |
022C: BSF 03.5 |
022D: BCF 06.5 |
022E: BCF 03.5 |
022F: BSF 06.5 |
.................... movement=R; |
0230: MOVLW 01 |
0231: MOVWF 45 |
.................... break; |
0232: GOTO 25D |
.................... case R: |
.................... FR;BL; |
0233: BSF 03.5 |
0234: BCF 06.5 |
0235: BCF 03.5 |
0236: BCF 06.5 |
0237: BSF 03.5 |
0238: BCF 06.4 |
0239: BCF 03.5 |
023A: BSF 06.4 |
023B: BSF 03.5 |
023C: BCF 06.6 |
023D: BCF 03.5 |
023E: BCF 06.6 |
023F: BSF 03.5 |
0240: BCF 06.7 |
0241: BCF 03.5 |
0242: BSF 06.7 |
.................... movement=L; |
0243: MOVLW 02 |
0244: MOVWF 45 |
.................... break; |
0245: GOTO 25D |
.................... case S: |
.................... FL;BR; |
0246: BSF 03.5 |
0247: BCF 06.7 |
0248: BCF 03.5 |
0249: BCF 06.7 |
024A: BSF 03.5 |
024B: BCF 06.6 |
024C: BCF 03.5 |
024D: BSF 06.6 |
024E: BSF 03.5 |
024F: BCF 06.4 |
0250: BCF 03.5 |
0251: BCF 06.4 |
0252: BSF 03.5 |
0253: BCF 06.5 |
0254: BCF 03.5 |
0255: BSF 06.5 |
.................... movement=R; |
0256: MOVLW 01 |
0257: MOVWF 45 |
.................... n=CIK_CAK/2; |
0258: MOVLW 3A |
0259: MOVWF 75 |
025A: MOVLW 98 |
025B: MOVWF 74 |
.................... break; |
025C: GOTO 25D |
025D: BSF 03.5 |
.................... } |
.................... } |
.................... n++; |
025E: INCF 74,F |
025F: BTFSC 03.2 |
0260: INCF 75,F |
.................... } |
0261: BCF 03.5 |
0262: GOTO 201 |
0263: BSF 03.5 |
.................... STOPL;STOPR; |
0264: BCF 06.6 |
0265: BCF 03.5 |
0266: BCF 06.6 |
0267: BSF 03.5 |
0268: BCF 06.7 |
0269: BCF 03.5 |
026A: BCF 06.7 |
026B: BSF 03.5 |
026C: BCF 06.4 |
026D: BCF 03.5 |
026E: BCF 06.4 |
026F: BSF 03.5 |
0270: BCF 06.5 |
0271: BCF 03.5 |
0272: BCF 06.5 |
.................... line = RSENSOR; // cteni senzoru na caru |
0273: CLRF 41 |
0274: BSF 03.5 |
0275: BTFSS 1C.7 |
0276: GOTO 27A |
0277: BCF 03.5 |
0278: INCF 41,F |
0279: BSF 03.5 |
.................... line |= LSENSOR << 1; |
027A: MOVLW 00 |
027B: BTFSC 1C.6 |
027C: MOVLW 01 |
027D: MOVWF 77 |
027E: BCF 03.0 |
027F: RLF 77,F |
0280: MOVF 77,W |
0281: BCF 03.5 |
0282: IORWF 41,F |
.................... if (line==0) goto sem1; |
0283: MOVF 41,F |
0284: BTFSC 03.2 |
0285: GOTO 1FD |
.................... // nasli jsme caru |
.................... line=S; |
0286: MOVLW 03 |
0287: MOVWF 41 |
.................... } |
0288: RETLW 00 |
.................... /////////////////////////////////////////////////////////////////////////////// |
.................... void objizdka() // objede cihlu |
.................... { |
.................... unsigned int16 n; |
.................... |
.................... BL;BR;Delay_ms(150); |
0289: BSF 03.5 |
028A: BCF 06.6 |
028B: BCF 03.5 |
028C: BCF 06.6 |
028D: BSF 03.5 |
028E: BCF 06.7 |
028F: BCF 03.5 |
0290: BSF 06.7 |
0291: BSF 03.5 |
0292: BCF 06.4 |
0293: BCF 03.5 |
0294: BCF 06.4 |
0295: BSF 03.5 |
0296: BCF 06.5 |
0297: BCF 03.5 |
0298: BSF 06.5 |
0299: MOVLW 96 |
029A: MOVWF 74 |
029B: CALL 088 |
.................... STOPR;STOPL; |
029C: BSF 03.5 |
029D: BCF 06.4 |
029E: BCF 03.5 |
029F: BCF 06.4 |
02A0: BSF 03.5 |
02A1: BCF 06.5 |
02A2: BCF 03.5 |
02A3: BCF 06.5 |
02A4: BSF 03.5 |
02A5: BCF 06.6 |
02A6: BCF 03.5 |
02A7: BCF 06.6 |
02A8: BSF 03.5 |
02A9: BCF 06.7 |
02AA: BCF 03.5 |
02AB: BCF 06.7 |
.................... beep(900,1000); |
02AC: MOVLW 03 |
02AD: MOVWF 75 |
02AE: MOVLW 84 |
02AF: MOVWF 74 |
02B0: MOVLW 03 |
02B1: MOVWF 7D |
02B2: MOVLW E8 |
02B3: MOVWF 7C |
02B4: CALL 05C |
.................... // movement=S; |
.................... // cikcak(); |
.................... |
.................... BR; FL; Delay_ms(270); // otoc se 70° do prava |
02B5: BSF 03.5 |
02B6: BCF 06.4 |
02B7: BCF 03.5 |
02B8: BCF 06.4 |
02B9: BSF 03.5 |
02BA: BCF 06.5 |
02BB: BCF 03.5 |
02BC: BSF 06.5 |
02BD: BSF 03.5 |
02BE: BCF 06.7 |
02BF: BCF 03.5 |
02C0: BCF 06.7 |
02C1: BSF 03.5 |
02C2: BCF 06.6 |
02C3: BCF 03.5 |
02C4: BSF 06.6 |
02C5: MOVLW 02 |
02C6: MOVWF 73 |
02C7: MOVLW 87 |
02C8: MOVWF 74 |
02C9: CALL 088 |
02CA: DECFSZ 73,F |
02CB: GOTO 2C7 |
.................... |
.................... FR; FL; Delay_ms(500); // popojed rovne |
02CC: BSF 03.5 |
02CD: BCF 06.5 |
02CE: BCF 03.5 |
02CF: BCF 06.5 |
02D0: BSF 03.5 |
02D1: BCF 06.4 |
02D2: BCF 03.5 |
02D3: BSF 06.4 |
02D4: BSF 03.5 |
02D5: BCF 06.7 |
02D6: BCF 03.5 |
02D7: BCF 06.7 |
02D8: BSF 03.5 |
02D9: BCF 06.6 |
02DA: BCF 03.5 |
02DB: BSF 06.6 |
02DC: MOVLW 02 |
02DD: MOVWF 73 |
02DE: MOVLW FA |
02DF: MOVWF 74 |
02E0: CALL 088 |
02E1: DECFSZ 73,F |
02E2: GOTO 2DE |
.................... |
.................... BL; Delay_ms(30); // otoc se 90° do leva |
02E3: BSF 03.5 |
02E4: BCF 06.6 |
02E5: BCF 03.5 |
02E6: BCF 06.6 |
02E7: BSF 03.5 |
02E8: BCF 06.7 |
02E9: BCF 03.5 |
02EA: BSF 06.7 |
02EB: MOVLW 1E |
02EC: MOVWF 74 |
02ED: CALL 088 |
.................... STOPL; FR; Delay_ms(500); |
02EE: BSF 03.5 |
02EF: BCF 06.6 |
02F0: BCF 03.5 |
02F1: BCF 06.6 |
02F2: BSF 03.5 |
02F3: BCF 06.7 |
02F4: BCF 03.5 |
02F5: BCF 06.7 |
02F6: BSF 03.5 |
02F7: BCF 06.5 |
02F8: BCF 03.5 |
02F9: BCF 06.5 |
02FA: BSF 03.5 |
02FB: BCF 06.4 |
02FC: BCF 03.5 |
02FD: BSF 06.4 |
02FE: MOVLW 02 |
02FF: MOVWF 73 |
0300: MOVLW FA |
0301: MOVWF 74 |
0302: CALL 088 |
0303: DECFSZ 73,F |
0304: GOTO 300 |
.................... |
.................... FR; FL; Delay_ms(100); // popojed rovne na slepo |
0305: BSF 03.5 |
0306: BCF 06.5 |
0307: BCF 03.5 |
0308: BCF 06.5 |
0309: BSF 03.5 |
030A: BCF 06.4 |
030B: BCF 03.5 |
030C: BSF 06.4 |
030D: BSF 03.5 |
030E: BCF 06.7 |
030F: BCF 03.5 |
0310: BCF 06.7 |
0311: BSF 03.5 |
0312: BCF 06.6 |
0313: BCF 03.5 |
0314: BSF 06.6 |
0315: MOVLW 64 |
0316: MOVWF 74 |
0317: CALL 088 |
.................... for(n=40000;n>0;n--) // popojed rovne ale kontroluj caru |
0318: MOVLW 9C |
0319: MOVWF 72 |
031A: MOVLW 40 |
031B: MOVWF 71 |
031C: MOVF 71,F |
031D: BTFSS 03.2 |
031E: GOTO 322 |
031F: MOVF 72,F |
0320: BTFSC 03.2 |
0321: GOTO 33E |
.................... { |
.................... line = RSENSOR; // cteni senzoru na caru |
0322: CLRF 41 |
0323: BSF 03.5 |
0324: BTFSS 1C.7 |
0325: GOTO 329 |
0326: BCF 03.5 |
0327: INCF 41,F |
0328: BSF 03.5 |
.................... line |= LSENSOR << 1; |
0329: MOVLW 00 |
032A: BTFSC 1C.6 |
032B: MOVLW 01 |
032C: MOVWF 77 |
032D: BCF 03.0 |
032E: RLF 77,F |
032F: MOVF 77,W |
0330: BCF 03.5 |
0331: IORWF 41,F |
.................... if (line!=0) {Delay_ms(150); break;} |
0332: MOVF 41,F |
0333: BTFSC 03.2 |
0334: GOTO 339 |
0335: MOVLW 96 |
0336: MOVWF 74 |
0337: CALL 088 |
0338: GOTO 33E |
.................... // Delay_ms(1); |
.................... } |
0339: MOVF 71,W |
033A: BTFSC 03.2 |
033B: DECF 72,F |
033C: DECF 71,F |
033D: GOTO 31C |
.................... |
.................... BR; FL; // otoc se 60° do prava |
033E: BSF 03.5 |
033F: BCF 06.4 |
0340: BCF 03.5 |
0341: BCF 06.4 |
0342: BSF 03.5 |
0343: BCF 06.5 |
0344: BCF 03.5 |
0345: BSF 06.5 |
0346: BSF 03.5 |
0347: BCF 06.7 |
0348: BCF 03.5 |
0349: BCF 06.7 |
034A: BSF 03.5 |
034B: BCF 06.6 |
034C: BCF 03.5 |
034D: BSF 06.6 |
.................... for(n=40000;n>0;n--) |
034E: MOVLW 9C |
034F: MOVWF 72 |
0350: MOVLW 40 |
0351: MOVWF 71 |
0352: MOVF 71,F |
0353: BTFSS 03.2 |
0354: GOTO 358 |
0355: MOVF 72,F |
0356: BTFSC 03.2 |
0357: GOTO 370 |
.................... { |
.................... line = RSENSOR; // cteni senzoru na caru |
0358: CLRF 41 |
0359: BSF 03.5 |
035A: BTFSS 1C.7 |
035B: GOTO 35F |
035C: BCF 03.5 |
035D: INCF 41,F |
035E: BSF 03.5 |
.................... line |= LSENSOR << 1; |
035F: MOVLW 00 |
0360: BTFSC 1C.6 |
0361: MOVLW 01 |
0362: MOVWF 77 |
0363: BCF 03.0 |
0364: RLF 77,F |
0365: MOVF 77,W |
0366: BCF 03.5 |
0367: IORWF 41,F |
.................... if (line!=0) break; |
0368: MOVF 41,F |
0369: BTFSS 03.2 |
036A: GOTO 370 |
.................... // Delay_ms(1); |
.................... } |
036B: MOVF 71,W |
036C: BTFSC 03.2 |
036D: DECF 72,F |
036E: DECF 71,F |
036F: GOTO 352 |
.................... STOPR; STOPL; |
0370: BSF 03.5 |
0371: BCF 06.4 |
0372: BCF 03.5 |
0373: BCF 06.4 |
0374: BSF 03.5 |
0375: BCF 06.5 |
0376: BCF 03.5 |
0377: BCF 06.5 |
0378: BSF 03.5 |
0379: BCF 06.6 |
037A: BCF 03.5 |
037B: BCF 06.6 |
037C: BSF 03.5 |
037D: BCF 06.7 |
037E: BCF 03.5 |
037F: BCF 06.7 |
.................... |
.................... movement=L; //R; |
0380: MOVLW 02 |
0381: MOVWF 45 |
.................... cikcak(); |
0382: CALL 1FD |
.................... dira=0; |
0383: CLRF 46 |
.................... } |
0384: BCF 0A.3 |
0385: GOTO 550 (RETURN) |
.................... /////////////////////////////////////////////////////////////////////////////// |
.................... void prejeddiru() // vyresi diru |
.................... { |
.................... unsigned int16 n; |
.................... unsigned int8 speed_dira; |
.................... |
.................... STOPL;STOPR; |
0386: BSF 03.5 |
0387: BCF 06.6 |
0388: BCF 03.5 |
0389: BCF 06.6 |
038A: BSF 03.5 |
038B: BCF 06.7 |
038C: BCF 03.5 |
038D: BCF 06.7 |
038E: BSF 03.5 |
038F: BCF 06.4 |
0390: BCF 03.5 |
0391: BCF 06.4 |
0392: BSF 03.5 |
0393: BCF 06.5 |
0394: BCF 03.5 |
0395: BCF 06.5 |
.................... speed_dira=speed; |
0396: MOVF 42,W |
0397: MOVWF 73 |
.................... beep(1000,500); |
0398: MOVLW 03 |
0399: MOVWF 75 |
039A: MOVLW E8 |
039B: MOVWF 74 |
039C: MOVLW 01 |
039D: MOVWF 7D |
039E: MOVLW F4 |
039F: MOVWF 7C |
03A0: CALL 05C |
.................... switch (movement) //vrat se zpet na caru |
.................... { |
03A1: MOVF 45,W |
03A2: XORLW 02 |
03A3: BTFSC 03.2 |
03A4: GOTO 3AC |
03A5: XORLW 03 |
03A6: BTFSC 03.2 |
03A7: GOTO 3E4 |
03A8: XORLW 02 |
03A9: BTFSC 03.2 |
03AA: GOTO 41C |
03AB: GOTO 41E |
.................... case L: |
.................... for (n=COUVANI;n>0;n--) {GO(R,B,speed_dira); Delay_ms(1);} |
03AC: MOVLW 02 |
03AD: MOVWF 72 |
03AE: MOVLW EE |
03AF: MOVWF 71 |
03B0: MOVF 71,F |
03B1: BTFSS 03.2 |
03B2: GOTO 3B6 |
03B3: MOVF 72,F |
03B4: BTFSC 03.2 |
03B5: GOTO 3D3 |
03B6: MOVF 01,W |
03B7: SUBWF 73,W |
03B8: BTFSS 03.0 |
03B9: GOTO 3C3 |
03BA: BSF 03.5 |
03BB: BCF 06.4 |
03BC: BCF 03.5 |
03BD: BCF 06.4 |
03BE: BSF 03.5 |
03BF: BCF 06.5 |
03C0: BCF 03.5 |
03C1: BSF 06.5 |
03C2: GOTO 3CB |
03C3: BSF 03.5 |
03C4: BCF 06.4 |
03C5: BCF 03.5 |
03C6: BCF 06.4 |
03C7: BSF 03.5 |
03C8: BCF 06.5 |
03C9: BCF 03.5 |
03CA: BCF 06.5 |
03CB: MOVLW 01 |
03CC: MOVWF 74 |
03CD: CALL 088 |
03CE: MOVF 71,W |
03CF: BTFSC 03.2 |
03D0: DECF 72,F |
03D1: DECF 71,F |
03D2: GOTO 3B0 |
.................... STOPL;STOPR; |
03D3: BSF 03.5 |
03D4: BCF 06.6 |
03D5: BCF 03.5 |
03D6: BCF 06.6 |
03D7: BSF 03.5 |
03D8: BCF 06.7 |
03D9: BCF 03.5 |
03DA: BCF 06.7 |
03DB: BSF 03.5 |
03DC: BCF 06.4 |
03DD: BCF 03.5 |
03DE: BCF 06.4 |
03DF: BSF 03.5 |
03E0: BCF 06.5 |
03E1: BCF 03.5 |
03E2: BCF 06.5 |
.................... break; |
03E3: GOTO 41E |
.................... case R: |
.................... for (n=COUVANI;n>0;n--) {GO(L,B,speed_dira); Delay_ms(1);} |
03E4: MOVLW 02 |
03E5: MOVWF 72 |
03E6: MOVLW EE |
03E7: MOVWF 71 |
03E8: MOVF 71,F |
03E9: BTFSS 03.2 |
03EA: GOTO 3EE |
03EB: MOVF 72,F |
03EC: BTFSC 03.2 |
03ED: GOTO 40B |
03EE: MOVF 01,W |
03EF: SUBWF 73,W |
03F0: BTFSS 03.0 |
03F1: GOTO 3FB |
03F2: BSF 03.5 |
03F3: BCF 06.6 |
03F4: BCF 03.5 |
03F5: BCF 06.6 |
03F6: BSF 03.5 |
03F7: BCF 06.7 |
03F8: BCF 03.5 |
03F9: BSF 06.7 |
03FA: GOTO 403 |
03FB: BSF 03.5 |
03FC: BCF 06.6 |
03FD: BCF 03.5 |
03FE: BCF 06.6 |
03FF: BSF 03.5 |
0400: BCF 06.7 |
0401: BCF 03.5 |
0402: BCF 06.7 |
0403: MOVLW 01 |
0404: MOVWF 74 |
0405: CALL 088 |
0406: MOVF 71,W |
0407: BTFSC 03.2 |
0408: DECF 72,F |
0409: DECF 71,F |
040A: GOTO 3E8 |
.................... STOPL;STOPR; |
040B: BSF 03.5 |
040C: BCF 06.6 |
040D: BCF 03.5 |
040E: BCF 06.6 |
040F: BSF 03.5 |
0410: BCF 06.7 |
0411: BCF 03.5 |
0412: BCF 06.7 |
0413: BSF 03.5 |
0414: BCF 06.4 |
0415: BCF 03.5 |
0416: BCF 06.4 |
0417: BSF 03.5 |
0418: BCF 06.5 |
0419: BCF 03.5 |
041A: BCF 06.5 |
.................... break; |
041B: GOTO 41E |
.................... case S: |
.................... goto sem; |
041C: GOTO 45C |
.................... break; |
041D: GOTO 41E |
.................... } |
.................... beep(1000,500); |
041E: MOVLW 03 |
041F: MOVWF 75 |
0420: MOVLW E8 |
0421: MOVWF 74 |
0422: MOVLW 01 |
0423: MOVWF 7D |
0424: MOVLW F4 |
0425: MOVWF 7C |
0426: CALL 05C |
.................... |
.................... /*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 |
0427: BSF 03.5 |
0428: BCF 06.5 |
0429: BCF 03.5 |
042A: BCF 06.5 |
042B: BSF 03.5 |
042C: BCF 06.4 |
042D: BCF 03.5 |
042E: BSF 06.4 |
042F: BSF 03.5 |
0430: BCF 06.7 |
0431: BCF 03.5 |
0432: BCF 06.7 |
0433: BSF 03.5 |
0434: BCF 06.6 |
0435: BCF 03.5 |
0436: BSF 06.6 |
.................... for(n=PRES_DIRU;n>0;n--) |
0437: MOVLW 01 |
0438: MOVWF 72 |
0439: MOVLW 2C |
043A: MOVWF 71 |
043B: MOVF 71,F |
043C: BTFSS 03.2 |
043D: GOTO 441 |
043E: MOVF 72,F |
043F: BTFSC 03.2 |
0440: GOTO 45C |
.................... { |
.................... line = RSENSOR; // cteni senzoru na caru |
0441: CLRF 41 |
0442: BSF 03.5 |
0443: BTFSS 1C.7 |
0444: GOTO 448 |
0445: BCF 03.5 |
0446: INCF 41,F |
0447: BSF 03.5 |
.................... line |= LSENSOR << 1; |
0448: MOVLW 00 |
0449: BTFSC 1C.6 |
044A: MOVLW 01 |
044B: MOVWF 77 |
044C: BCF 03.0 |
044D: RLF 77,F |
044E: MOVF 77,W |
044F: BCF 03.5 |
0450: IORWF 41,F |
.................... if (line!=0) break; |
0451: MOVF 41,F |
0452: BTFSS 03.2 |
0453: GOTO 45C |
.................... Delay_ms(1); |
0454: MOVLW 01 |
0455: MOVWF 74 |
0456: CALL 088 |
.................... } |
0457: MOVF 71,W |
0458: BTFSC 03.2 |
0459: DECF 72,F |
045A: DECF 71,F |
045B: GOTO 43B |
.................... sem: |
.................... STOPL; STOPR; |
045C: BSF 03.5 |
045D: BCF 06.6 |
045E: BCF 03.5 |
045F: BCF 06.6 |
0460: BSF 03.5 |
0461: BCF 06.7 |
0462: BCF 03.5 |
0463: BCF 06.7 |
0464: BSF 03.5 |
0465: BCF 06.4 |
0466: BCF 03.5 |
0467: BCF 06.4 |
0468: BSF 03.5 |
0469: BCF 06.5 |
046A: BCF 03.5 |
046B: BCF 06.5 |
.................... movement=S; |
046C: MOVLW 03 |
046D: MOVWF 45 |
.................... cikcak(); // najdi caru |
046E: CALL 1FD |
.................... dira=0; |
046F: CLRF 46 |
.................... } |
0470: BCF 0A.3 |
0471: GOTO 5E6 (RETURN) |
.................... /////////////////////////////////////////////////////////////////////////////// |
.................... void main() |
.................... { |
0472: CLRF 04 |
0473: MOVLW 1F |
0474: ANDWF 03,F |
0475: BSF 03.5 |
0476: BCF 1F.4 |
0477: BCF 1F.5 |
0478: MOVF 1B,W |
0479: ANDLW 80 |
047A: MOVWF 1B |
047B: MOVLW 07 |
047C: MOVWF 1C |
047D: MOVF 1C,W |
047E: BCF 03.5 |
047F: BCF 0D.6 |
0480: MOVLW 60 |
0481: BSF 03.5 |
0482: MOVWF 0F |
.................... unsigned int16 n; // pro FOR |
.................... |
.................... STOPL; STOPR; // prepne vystupy na ovladani motoru na output a zastavi |
* |
049F: BSF 03.5 |
04A0: BCF 06.6 |
04A1: BCF 03.5 |
04A2: BCF 06.6 |
04A3: BSF 03.5 |
04A4: BCF 06.7 |
04A5: BCF 03.5 |
04A6: BCF 06.7 |
04A7: BSF 03.5 |
04A8: BCF 06.4 |
04A9: BCF 03.5 |
04AA: BCF 06.4 |
04AB: BSF 03.5 |
04AC: BCF 06.5 |
04AD: BCF 03.5 |
04AE: BCF 06.5 |
.................... |
.................... setup_oscillator(OSC_4MHZ|OSC_INTRC); // 4 MHz interni RC oscilator |
04AF: MOVLW 62 |
04B0: BSF 03.5 |
04B1: MOVWF 0F |
.................... |
.................... port_b_pullups(TRUE); // pullups pro piano na diagnostiku |
04B2: BCF 01.7 |
.................... setup_spi(FALSE); |
04B3: BCF 03.5 |
04B4: BCF 14.5 |
04B5: BSF 03.5 |
04B6: BCF 06.2 |
04B7: BSF 06.1 |
04B8: BCF 06.4 |
04B9: MOVLW 00 |
04BA: BCF 03.5 |
04BB: MOVWF 14 |
04BC: BSF 03.5 |
04BD: MOVWF 14 |
.................... setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro PWM |
04BE: MOVF 01,W |
04BF: ANDLW C7 |
04C0: IORLW 08 |
04C1: MOVWF 01 |
.................... |
.................... setup_timer_2(T2_DIV_BY_4,255,10); // Casovac pro regulaci |
04C2: MOVLW 48 |
04C3: MOVWF 78 |
04C4: IORLW 05 |
04C5: BCF 03.5 |
04C6: MOVWF 12 |
04C7: MOVLW FF |
04C8: BSF 03.5 |
04C9: MOVWF 12 |
.................... // preruseni kazdych 10ms |
.................... setup_adc_ports(sAN2|VSS_VDD); // nastaveni A/D prevodniku pro naraznik |
04CA: BCF 1F.4 |
04CB: BCF 1F.5 |
04CC: MOVF 1B,W |
04CD: ANDLW 80 |
04CE: IORLW 04 |
04CF: MOVWF 1B |
.................... setup_adc(ADC_CLOCK_INTERNAL); |
04D0: BCF 1F.6 |
04D1: BCF 03.5 |
04D2: BSF 1F.6 |
04D3: BSF 1F.7 |
04D4: BSF 03.5 |
04D5: BCF 1F.7 |
04D6: BCF 03.5 |
04D7: BSF 1F.0 |
.................... set_adc_channel(2); |
04D8: MOVLW 10 |
04D9: MOVWF 78 |
04DA: MOVF 1F,W |
04DB: ANDLW C7 |
04DC: IORWF 78,W |
04DD: MOVWF 1F |
.................... setup_timer_1(T1_INTERNAL|T1_DIV_BY_1); // Casovac pro naraznik |
04DE: MOVLW 85 |
04DF: MOVWF 10 |
.................... setup_ccp1(CCP_COMPARE_RESET_TIMER); |
04E0: BSF 03.5 |
04E1: BSF 06.3 |
04E2: MOVLW 0B |
04E3: BCF 03.5 |
04E4: MOVWF 17 |
.................... CCP_1=(2^10)-1; // prevod kazdou 1ms |
04E5: CLRF 16 |
04E6: MOVLW 07 |
04E7: MOVWF 15 |
.................... |
.................... setup_comparator(A0_VR_A1_VR); // inicializace komparatoru pro cidla cary |
04E8: MOVLW 02 |
04E9: BSF 03.5 |
04EA: MOVWF 1C |
04EB: MOVF 05,W |
04EC: IORLW 03 |
04ED: MOVWF 05 |
04EE: MOVLW 03 |
04EF: MOVWF 77 |
04F0: DECFSZ 77,F |
04F1: GOTO 4F0 |
04F2: MOVF 1C,W |
04F3: BCF 03.5 |
04F4: BCF 0D.6 |
.................... setup_vref(VREF_HIGH|TRESHOLD); // 32 kroku od 0.25 do 0.75 Vdd |
04F5: MOVLW 86 |
04F6: BSF 03.5 |
04F7: MOVWF 1D |
.................... |
.................... Beep(1000,200); //double beep |
04F8: MOVLW 03 |
04F9: MOVWF 75 |
04FA: MOVLW E8 |
04FB: MOVWF 74 |
04FC: CLRF 7D |
04FD: MOVLW C8 |
04FE: MOVWF 7C |
04FF: BCF 03.5 |
0500: CALL 05C |
.................... Delay_ms(50); |
0501: MOVLW 32 |
0502: MOVWF 74 |
0503: CALL 088 |
.................... Beep(1000,200); |
0504: MOVLW 03 |
0505: MOVWF 75 |
0506: MOVLW E8 |
0507: MOVWF 74 |
0508: CLRF 7D |
0509: MOVLW C8 |
050A: MOVWF 7C |
050B: CALL 05C |
.................... Delay_ms(1000); // 1s |
050C: MOVLW 04 |
050D: MOVWF 71 |
050E: MOVLW FA |
050F: MOVWF 74 |
0510: CALL 088 |
0511: DECFSZ 71,F |
0512: GOTO 50E |
.................... |
.................... // povoleni rizeni rychlosti zataceni pres preruseni |
.................... enable_interrupts(INT_TIMER2); |
0513: BSF 03.5 |
0514: BSF 0C.1 |
.................... enable_interrupts(GLOBAL); |
0515: MOVLW C0 |
0516: BCF 03.5 |
0517: IORWF 0B,F |
.................... |
.................... /*---------------------------------------------------------------------------*/ |
.................... sensors=S; |
0518: MOVLW 03 |
0519: MOVWF 40 |
.................... line=S; |
051A: MOVWF 41 |
.................... last=S; |
051B: MOVWF 44 |
.................... movement=S; |
051C: MOVWF 45 |
.................... speed=FW_POMALU; |
051D: MOVLW E6 |
051E: MOVWF 42 |
.................... |
.................... diagnostika(); |
051F: GOTO 09D |
.................... //cikcak(); // toc se, abys nasel caru |
.................... Delay_ms(500); |
0520: MOVLW 02 |
0521: MOVWF 71 |
0522: MOVLW FA |
0523: MOVWF 74 |
0524: CALL 088 |
0525: DECFSZ 71,F |
0526: GOTO 522 |
.................... Beep(1000,200); |
0527: MOVLW 03 |
0528: MOVWF 75 |
0529: MOVLW E8 |
052A: MOVWF 74 |
052B: CLRF 7D |
052C: MOVLW C8 |
052D: MOVWF 7C |
052E: CALL 05C |
.................... Delay_ms(500); |
052F: MOVLW 02 |
0530: MOVWF 71 |
0531: MOVLW FA |
0532: MOVWF 74 |
0533: CALL 088 |
0534: DECFSZ 71,F |
0535: GOTO 531 |
.................... |
.................... while(true) // hlavni smycka (jizda podle cary) |
.................... { |
.................... sensors = RSENSOR; // cteni senzoru na caru |
0536: CLRF 40 |
0537: BSF 03.5 |
0538: BTFSS 1C.7 |
0539: GOTO 53D |
053A: BCF 03.5 |
053B: INCF 40,F |
053C: BSF 03.5 |
.................... sensors |= LSENSOR << 1; |
053D: MOVLW 00 |
053E: BTFSC 1C.6 |
053F: MOVLW 01 |
0540: MOVWF 77 |
0541: BCF 03.0 |
0542: RLF 77,F |
0543: MOVF 77,W |
0544: BCF 03.5 |
0545: IORWF 40,F |
.................... |
.................... if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD) && (dira<=T_CIHLA)) objizdka(); |
0546: BTFSC 1F.2 |
0547: GOTO 546 |
0548: MOVF 1E,W |
0549: SUBLW 7F |
054A: BTFSS 03.0 |
054B: GOTO 550 |
054C: MOVF 46,W |
054D: SUBLW 32 |
054E: BTFSC 03.0 |
054F: GOTO 289 |
.................... |
.................... switch (sensors) // zatacej podle toho, kde vidis caru |
.................... { |
0550: MOVF 40,W |
0551: XORLW 03 |
0552: BTFSC 03.2 |
0553: GOTO 55B |
0554: XORLW 01 |
0555: BTFSC 03.2 |
0556: GOTO 56F |
0557: XORLW 03 |
0558: BTFSC 03.2 |
0559: GOTO 5A8 |
055A: GOTO 5E1 |
.................... case S: // rovne |
.................... FL; FR; // pokud se jede dlouho rovne, tak pridej |
055B: BSF 03.5 |
055C: BCF 06.7 |
055D: BCF 03.5 |
055E: BCF 06.7 |
055F: BSF 03.5 |
0560: BCF 06.6 |
0561: BCF 03.5 |
0562: BSF 06.6 |
0563: BSF 03.5 |
0564: BCF 06.5 |
0565: BCF 03.5 |
0566: BCF 06.5 |
0567: BSF 03.5 |
0568: BCF 06.4 |
0569: BCF 03.5 |
056A: BSF 06.4 |
.................... dira=0; |
056B: CLRF 46 |
.................... movement=S; |
056C: MOVLW 03 |
056D: MOVWF 45 |
.................... continue; |
056E: GOTO 536 |
.................... case L: // trochu vlevo |
.................... GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka); |
056F: MOVF 01,W |
0570: MOVWF 71 |
0571: MOVLW E6 |
0572: ADDWF 43,W |
0573: SUBWF 71,W |
0574: BTFSC 03.2 |
0575: GOTO 578 |
0576: BTFSC 03.0 |
0577: GOTO 581 |
0578: BSF 03.5 |
0579: BCF 06.7 |
057A: BCF 03.5 |
057B: BCF 06.7 |
057C: BSF 03.5 |
057D: BCF 06.6 |
057E: BCF 03.5 |
057F: BSF 06.6 |
0580: GOTO 589 |
0581: BSF 03.5 |
0582: BCF 06.6 |
0583: BCF 03.5 |
0584: BCF 06.6 |
0585: BSF 03.5 |
0586: BCF 06.7 |
0587: BCF 03.5 |
0588: BCF 06.7 |
0589: MOVF 01,W |
058A: MOVWF 71 |
058B: MOVLW F0 |
058C: ADDWF 43,W |
058D: SUBWF 71,W |
058E: BTFSC 03.2 |
058F: GOTO 592 |
0590: BTFSC 03.0 |
0591: GOTO 59B |
0592: BSF 03.5 |
0593: BCF 06.5 |
0594: BCF 03.5 |
0595: BCF 06.5 |
0596: BSF 03.5 |
0597: BCF 06.4 |
0598: BCF 03.5 |
0599: BSF 06.4 |
059A: GOTO 5A3 |
059B: BSF 03.5 |
059C: BCF 06.4 |
059D: BCF 03.5 |
059E: BCF 06.4 |
059F: BSF 03.5 |
05A0: BCF 06.5 |
05A1: BCF 03.5 |
05A2: BCF 06.5 |
.................... line=L; |
05A3: MOVLW 02 |
05A4: MOVWF 41 |
.................... dira=0; |
05A5: CLRF 46 |
.................... movement=L; |
05A6: MOVWF 45 |
.................... continue; |
05A7: GOTO 536 |
.................... case R: // trochu vpravo |
.................... GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka); |
05A8: MOVF 01,W |
05A9: MOVWF 71 |
05AA: MOVLW E6 |
05AB: ADDWF 43,W |
05AC: SUBWF 71,W |
05AD: BTFSC 03.2 |
05AE: GOTO 5B1 |
05AF: BTFSC 03.0 |
05B0: GOTO 5BA |
05B1: BSF 03.5 |
05B2: BCF 06.5 |
05B3: BCF 03.5 |
05B4: BCF 06.5 |
05B5: BSF 03.5 |
05B6: BCF 06.4 |
05B7: BCF 03.5 |
05B8: BSF 06.4 |
05B9: GOTO 5C2 |
05BA: BSF 03.5 |
05BB: BCF 06.4 |
05BC: BCF 03.5 |
05BD: BCF 06.4 |
05BE: BSF 03.5 |
05BF: BCF 06.5 |
05C0: BCF 03.5 |
05C1: BCF 06.5 |
05C2: MOVF 01,W |
05C3: MOVWF 71 |
05C4: MOVLW F0 |
05C5: ADDWF 43,W |
05C6: SUBWF 71,W |
05C7: BTFSC 03.2 |
05C8: GOTO 5CB |
05C9: BTFSC 03.0 |
05CA: GOTO 5D4 |
05CB: BSF 03.5 |
05CC: BCF 06.7 |
05CD: BCF 03.5 |
05CE: BCF 06.7 |
05CF: BSF 03.5 |
05D0: BCF 06.6 |
05D1: BCF 03.5 |
05D2: BSF 06.6 |
05D3: GOTO 5DC |
05D4: BSF 03.5 |
05D5: BCF 06.6 |
05D6: BCF 03.5 |
05D7: BCF 06.6 |
05D8: BSF 03.5 |
05D9: BCF 06.7 |
05DA: BCF 03.5 |
05DB: BCF 06.7 |
.................... line=R; |
05DC: MOVLW 01 |
05DD: MOVWF 41 |
.................... dira=0; |
05DE: CLRF 46 |
.................... movement=R; |
05DF: MOVWF 45 |
.................... continue; |
05E0: GOTO 536 |
.................... default: // kdyz jsou obe cidla mimo caru, tak pokracuj dal |
.................... } |
.................... rovinka=0; |
05E1: CLRF 43 |
.................... if (dira>=T_DIRA) prejeddiru(); |
05E2: MOVF 46,W |
05E3: SUBLW 77 |
05E4: BTFSS 03.0 |
05E5: GOTO 386 |
.................... if (last!=line) // pokud si prejel caru z jedne strany na druhou stranu, tak zabrzdi |
05E6: MOVF 41,W |
05E7: SUBWF 44,W |
05E8: BTFSC 03.2 |
05E9: GOTO 5EE |
.................... { |
.................... last=line; |
05EA: MOVF 41,W |
05EB: MOVWF 44 |
.................... speed=FW_ZATACKA; |
05EC: MOVLW F0 |
05ED: MOVWF 42 |
.................... } |
.................... if (L==line) // kdyz jsou obe cidla mimo caru, zatoc na caru |
05EE: MOVF 41,W |
05EF: SUBLW 02 |
05F0: BTFSS 03.2 |
05F1: GOTO 612 |
.................... { |
.................... STOPL; |
05F2: BSF 03.5 |
05F3: BCF 06.6 |
05F4: BCF 03.5 |
05F5: BCF 06.6 |
05F6: BSF 03.5 |
05F7: BCF 06.7 |
05F8: BCF 03.5 |
05F9: BCF 06.7 |
.................... GO(R, F, speed); |
05FA: MOVF 01,W |
05FB: SUBWF 42,W |
05FC: BTFSS 03.0 |
05FD: GOTO 607 |
05FE: BSF 03.5 |
05FF: BCF 06.5 |
0600: BCF 03.5 |
0601: BCF 06.5 |
0602: BSF 03.5 |
0603: BCF 06.4 |
0604: BCF 03.5 |
0605: BSF 06.4 |
0606: GOTO 60F |
0607: BSF 03.5 |
0608: BCF 06.4 |
0609: BCF 03.5 |
060A: BCF 06.4 |
060B: BSF 03.5 |
060C: BCF 06.5 |
060D: BCF 03.5 |
060E: BCF 06.5 |
.................... movement=L; |
060F: MOVLW 02 |
0610: MOVWF 45 |
.................... } |
.................... else |
0611: GOTO 631 |
.................... { |
.................... STOPR; |
0612: BSF 03.5 |
0613: BCF 06.4 |
0614: BCF 03.5 |
0615: BCF 06.4 |
0616: BSF 03.5 |
0617: BCF 06.5 |
0618: BCF 03.5 |
0619: BCF 06.5 |
.................... GO(L, F, speed); |
061A: MOVF 01,W |
061B: SUBWF 42,W |
061C: BTFSS 03.0 |
061D: GOTO 627 |
061E: BSF 03.5 |
061F: BCF 06.7 |
0620: BCF 03.5 |
0621: BCF 06.7 |
0622: BSF 03.5 |
0623: BCF 06.6 |
0624: BCF 03.5 |
0625: BSF 06.6 |
0626: GOTO 62F |
0627: BSF 03.5 |
0628: BCF 06.6 |
0629: BCF 03.5 |
062A: BCF 06.6 |
062B: BSF 03.5 |
062C: BCF 06.7 |
062D: BCF 03.5 |
062E: BCF 06.7 |
.................... movement=R; |
062F: MOVLW 01 |
0630: MOVWF 45 |
.................... } |
.................... } // while(true) |
0631: GOTO 536 |
.................... } |
.................... |
.................... |
0632: SLEEP |
Configuration Fuses: |
Word 1: 3F38 NOWDT NOPUT MCLR NOBROWNOUT NOLVP NOCPD NOWRT NODEBUG CCPB0 NOPROTECT INTRC_IO |
Word 2: 3FFC NOFCMEN NOIESO |
/roboti/istrobot/merkur/PIC16F88/turn_R/tank.PJT |
---|
0,0 → 1,40 |
[PROJECT] |
Target=tank.HEX |
Development_Mode= |
Processor=0x688F |
ToolSuite=CCS |
[Directories] |
Include=C:\Program Files\PICC\devices\;C:\Program Files\PICC\drivers\;C:\library\CCS; |
Library= |
LinkerScript= |
[Target Data] |
FileList=tank.c; |
BuildTool=C-COMPILER |
OptionString=+FM |
AdditionalOptionString= |
BuildRequired=1 |
[tank.c] |
Type=4 |
Path= |
FileList= |
BuildTool= |
OptionString= |
AdditionalOptionString= |
[mru-list] |
1=tank.c |
[Windows] |
0=0000 tank.c 0 0 796 451 3 0 |
[Opened Files] |
1=D:\KAKLIK\programy\PIC_C\roboti\merkur\PIC16F88\turn_R\tank.c |
2=C:\Program Files\PICC\devices\16F88.h |
3= |
4=C:\Program Files\PICC\devices\16F88.h |
5= |
6= |
7= |
/roboti/istrobot/merkur/PIC16F88/turn_R/tank.SYM |
---|
0,0 → 1,88 |
003 STATUS |
015-016 CCP_1 |
015 CCP_1_LOW |
016 CCP_1_HIGH |
020 @INTERRUPT_AREA |
021 @INTERRUPT_AREA |
022 @INTERRUPT_AREA |
023 @INTERRUPT_AREA |
024 @INTERRUPT_AREA |
025 @INTERRUPT_AREA |
026 @INTERRUPT_AREA |
027 @INTERRUPT_AREA |
028 @INTERRUPT_AREA |
029-038 SendData |
039.0 bit |
03A fcslo |
03B fcshi |
03C stuff |
03D flag_flag |
03E fcs_flag |
03F i |
040 sensors |
041 line |
042 speed |
043 rovinka |
044 last |
045 movement |
046 dira |
047-06E AXstring |
06F-070 main.n |
071-072 objizdka.n |
071-072 diagnostika.n |
071-072 prejeddiru.n |
071 main.@SCRATCH |
073 prejeddiru.speed_dira |
073 diagnostika.@SCRATCH |
073 objizdka.@SCRATCH |
074-075 beep.period |
074 @delay_ms1.P1 |
074-075 cikcak.n |
074 prejeddiru.@SCRATCH |
076 cikcak.@SCRATCH |
077 @SCRATCH |
078 @SCRATCH |
078 _RETURN_ |
079 @SCRATCH |
07A @SCRATCH |
07B @SCRATCH |
07C-07D beep.length |
09C.6 C1OUT |
09C.7 C2OUT |
0A0-0A1 beep.nn |
0A2 @delay_us1.P1 |
0088 @delay_ms1 |
0047 @delay_us1 |
0037 TIMER2_isr |
005C beep |
009D diagnostika |
01FD cikcak |
0289 objizdka |
0386 prejeddiru |
0472 main |
0472 @cinit |
01FD sem1 |
045C sem |
Project Files: |
D:\KAKLIK\programy\PIC_C\roboti\merkur\PIC16F88\turn_R\tank.c |
D:\KAKLIK\programy\PIC_C\roboti\merkur\PIC16F88\turn_R\tank.h |
C:\Program Files\PICC\devices\16F88.h |
D:\KAKLIK\programy\PIC_C\roboti\merkur\PIC16F88\turn_R\AX25.c |
Compiler Settings: |
Processor: PIC16F88 |
Pointer Size: 8 |
ADC Range: 0-255 |
Opt Level: 9 |
Short,Int,Long: 1,8,16 |
Output Files: |
Errors: D:\KAKLIK\programy\PIC_C\roboti\merkur\PIC16F88\turn_R\tank.err |
INHX8: D:\KAKLIK\programy\PIC_C\roboti\merkur\PIC16F88\turn_R\tank.HEX |
Symbols: D:\KAKLIK\programy\PIC_C\roboti\merkur\PIC16F88\turn_R\tank.SYM |
List: D:\KAKLIK\programy\PIC_C\roboti\merkur\PIC16F88\turn_R\tank.LST |
Debug/COFF: D:\KAKLIK\programy\PIC_C\roboti\merkur\PIC16F88\turn_R\tank.cof |
Call Tree: D:\KAKLIK\programy\PIC_C\roboti\merkur\PIC16F88\turn_R\tank.tre |
Statistics: D:\KAKLIK\programy\PIC_C\roboti\merkur\PIC16F88\turn_R\tank.sta |
/roboti/istrobot/merkur/PIC16F88/turn_R/tank.c |
---|
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(500); // 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) |
} |
/roboti/istrobot/merkur/PIC16F88/turn_R/tank.cof |
---|
Cannot display: file marked as a binary type. |
svn:mime-type = application/octet-stream |
Property changes: |
Added: svn:mime-type |
+application/octet-stream |
\ No newline at end of property |
/roboti/istrobot/merkur/PIC16F88/turn_R/tank.err |
---|
0,0 → 1,0 |
No Errors |
/roboti/istrobot/merkur/PIC16F88/turn_R/tank.h |
---|
0,0 → 1,5 |
#include <16F88.h> |
#device adc=8 |
#fuses NOWDT,INTRC_IO, NOPUT, MCLR, NOBROWNOUT, NOLVP, NOCPD, NOWRT, NODEBUG, NOPROTECT, NOFCMEN, NOIESO |
#use delay(clock=4000000) |
/roboti/istrobot/merkur/PIC16F88/turn_R/tank.sta |
---|
0,0 → 1,38 |
ROM used: 1587 (39%) |
1587 (39%) including unused fragments |
2 Average locations per line |
4 Average locations per statement |
RAM used: 87 (50%) at main() level |
98 (56%) worst case |
Lines Stmts % Files |
----- ----- --- ----- |
340 357 84 D:\KAKLIK\programy\PIC_C\roboti\merkur\PIC16F88\turn_R\tank.c |
6 0 0 D:\KAKLIK\programy\PIC_C\roboti\merkur\PIC16F88\turn_R\tank.h |
275 0 0 C:\Program Files\PICC\devices\16F88.h |
136 67 9 D:\KAKLIK\programy\PIC_C\roboti\merkur\PIC16F88\turn_R\AX25.c |
----- ----- |
1514 848 Total |
Page ROM % RAM Functions: |
---- --- --- --- ---------- |
0 21 1 1 @delay_ms1 |
0 21 1 1 @delay_us1 |
0 16 1 0 TIMER2_isr |
0 44 3 6 beep |
0 352 22 3 diagnostika |
0 140 9 3 cikcak |
0 253 16 3 objizdka |
0 236 15 4 prejeddiru |
0 449 28 3 main |
Segment Used Free |
--------- ---- ---- |
00000-00003 4 0 |
00004-00036 51 0 |
00037-007FF 1532 461 |
00800-00FFF 0 2048 |
/roboti/istrobot/merkur/PIC16F88/turn_R/tank.tre |
---|
0,0 → 1,74 |
ÀÄtank |
ÃÄmain 0/449 Ram=3 |
³ ÃÄ??0?? |
³ ÃÄbeep 0/44 Ram=6 |
³ ³ ÃÄ@delay_us1 0/21 Ram=1 |
³ ³ ÀÄ@delay_us1 0/21 Ram=1 |
³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ÃÄbeep 0/44 Ram=6 |
³ ³ ÃÄ@delay_us1 0/21 Ram=1 |
³ ³ ÀÄ@delay_us1 0/21 Ram=1 |
³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ÃÄdiagnostika 0/352 Ram=3 |
³ ³ ÃÄbeep 0/44 Ram=6 |
³ ³ ³ ÃÄ@delay_us1 0/21 Ram=1 |
³ ³ ³ ÀÄ@delay_us1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄbeep 0/44 Ram=6 |
³ ³ ³ ÃÄ@delay_us1 0/21 Ram=1 |
³ ³ ³ ÀÄ@delay_us1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄbeep 0/44 Ram=6 |
³ ³ ³ ÃÄ@delay_us1 0/21 Ram=1 |
³ ³ ³ ÀÄ@delay_us1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄbeep 0/44 Ram=6 |
³ ³ ³ ÃÄ@delay_us1 0/21 Ram=1 |
³ ³ ³ ÀÄ@delay_us1 0/21 Ram=1 |
³ ³ ÃÄbeep 0/44 Ram=6 |
³ ³ ³ ÃÄ@delay_us1 0/21 Ram=1 |
³ ³ ³ ÀÄ@delay_us1 0/21 Ram=1 |
³ ³ ÀÄbeep 0/44 Ram=6 |
³ ³ ÃÄ@delay_us1 0/21 Ram=1 |
³ ³ ÀÄ@delay_us1 0/21 Ram=1 |
³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ÃÄbeep 0/44 Ram=6 |
³ ³ ÃÄ@delay_us1 0/21 Ram=1 |
³ ³ ÀÄ@delay_us1 0/21 Ram=1 |
³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ÃÄobjizdka 0/253 Ram=3 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄbeep 0/44 Ram=6 |
³ ³ ³ ÃÄ@delay_us1 0/21 Ram=1 |
³ ³ ³ ÀÄ@delay_us1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ³ ÀÄcikcak 0/140 Ram=3 |
³ ÀÄprejeddiru 0/236 Ram=4 |
³ ÃÄbeep 0/44 Ram=6 |
³ ³ ÃÄ@delay_us1 0/21 Ram=1 |
³ ³ ÀÄ@delay_us1 0/21 Ram=1 |
³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ÃÄbeep 0/44 Ram=6 |
³ ³ ÃÄ@delay_us1 0/21 Ram=1 |
³ ³ ÀÄ@delay_us1 0/21 Ram=1 |
³ ÃÄ@delay_ms1 0/21 Ram=1 |
³ ÀÄcikcak 0/140 Ram=3 |
ÀÄTIMER2_isr 0/16 Ram=0 |
/roboti/istrobot/merkur/PIC16F88/turn_R/verze/1 tank.c |
---|
0,0 → 1,313 |
#include "tank.h" |
#define TXo PIN_B1 // To the transmitter modulator |
#include "AX25.c" // podprogram pro prenos telemetrie |
//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) |
#define STOPL output_low(PIN_B6);output_low(PIN_B7) |
#define L 0b10 // left |
#define R 0b01 // right |
#define S 0b11 // straight |
#define COUVANI 1600 // couvnuti po zjisteni diry |
#define MEZERA 5400 // za jak dlouho bude ztracena cara |
#define PRES_DIRU 1000 // velikost mezery v care |
#define BRZDENI 5000 // doba ptrebna k zastaveni jednoho motoru |
//cidla |
#define RSENSOR C2OUT // Senzory na caru |
#define LSENSOR C1OUT |
#define BUMPER PIN_A4 // sensor na cihlu |
#define DIAG_SERVO PIN_B2 // Propojka pro diagnosticky mod |
#define DIAG_SENSORS PIN_B3 // Propojka pro diagnosticky mod |
#DEFINE SOUND_HI PIN_A7 |
#DEFINE SOUND_LO PIN_A6 |
char AXstring[40]; // Buffer pro prenos telemetrie |
#define GO(motor, direction, power) if(get_timer0()<=power) \ |
{direction##motor;} \ |
else \ |
{stop##motor;} |
int movement; // smer minuleho pohybu |
int line; // na ktere strane byla detekovana cara |
unsigned int16 dira; // pocitadlo pro nalezeni preruseni cary |
int speed,speedL,speedR; |
// 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); |
} |
} |
#int_TIMER2 |
void TIMER2_isr() |
{ |
switch(line) // upravime smer |
{ |
case S: //obe cidla na care |
if(speedL<200)speedL++; |
if(speedR<200)speedR++; |
break; // vrat se zpet na cteni cidel |
case L: // cara je pod levym cidlem, trochu zatocime |
if (speedL>100)speedL -- ; |
if (speedR<200)speedR ++ ; |
break; |
case R: // cara pod pravym cidlem |
if (speedR>100)speedR -- ; |
if (speedL<200)speedL ++ ; |
break; |
default: |
} |
} |
// Diagnostika pohonu, hejbne vsema motorama ve vsech smerech |
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(); |
sprintf(AXstring,"L: %U R: %U\0", ls, rs); // Convert DATA to String. |
SendPacket(&AXstring[0]); |
delay_ms(1000); |
}; |
} |
void cikcak() |
{ |
int n; |
switch(movement) // podivej se na jednu stranu |
{ |
case L: |
FL;BR; |
movement=R; |
break; |
case R: |
FR;BL; |
movement=L; |
break; |
case S: |
FR;BL; |
movement=L; |
break; |
} |
while (0==(RSENSOR|LSENSOR)) |
{ |
if (n==50) // asi bude na druhe strane |
{ |
STOPR;STOPL; |
n=0; |
switch(movement) |
{ |
case L: |
FL;BR; |
movement=R; |
break; |
case R: |
FR;BL; |
movement=L; |
break; |
} |
} |
Delay_ms(5); |
n++; |
} |
STOPL;STOPR; // nasli jsme caru |
line=S; |
} |
void objizdka() |
{ |
BL;BR;Delay_ms(300); |
STOPR;STOPL; |
beep(1000,1000); |
Delay_ms(500); |
beep(1000,1000); |
Delay_ms(1000); |
} |
void pozordira() |
{ |
beep(800,500); |
Delay_ms(50); |
beep(800,500); |
switch (movement) //vrat se zpet na caru |
{ |
case L: |
STOPL;STOPR; |
BR;Delay_ms(COUVANI);STOPR; |
break; |
case R: |
STOPL;STOPR; |
BL;Delay_ms(COUVANI);STOPL; |
break; |
case S: |
BL; BR; Delay_ms(COUVANI); |
STOPL; STOPR; |
break; |
} |
FR;FL; Delay_ms(PRES_DIRU); // popojedem dopredu mozna tam bude cara |
STOPL; STOPR; movement=S; |
cikcak(); // najdi caru |
dira=0; |
} |
void main() |
{ |
unsigned int16 rovinka; |
int last; |
STOPL; STOPR; |
setup_oscillator(OSC_4MHZ|OSC_INTRC); // 4 MHz interni RC oscilator |
port_b_pullups(true); |
setup_adc_ports(NO_ANALOGS|VSS_VDD); |
setup_adc(ADC_OFF); |
setup_spi(FALSE); |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro PWM |
setup_timer_1(T1_DISABLED); // Casovac pro regulaci |
setup_timer_2(T2_DIV_BY_16,50,16); |
setup_ccp1(CCP_OFF); |
setup_comparator(A0_VR_A1_VR); |
setup_vref(VREF_HIGH|15); |
enable_interrupts(INT_TIMER2); |
enable_interrupts(GLOBAL); |
diagnostika(); |
Beep(1000,200); //double beep |
Delay_ms(50); |
Beep(1000,200); |
Delay_ms(1000); // 1s |
// FL; FR; |
movement=S; |
line=S; |
dira=0; |
last=0; |
rovinka=0; |
speed=speedL=speedR=200; |
while(true) |
{ |
line = RSENSOR; // precteni cidel |
line |= LSENSOR << 1; // sestav informaci o care |
if(line!=0) |
{ |
GO(L, F, speedL); GO(R,F, speedR); |
} |
else{STOPR; STOPL;} |
//sem: |
/* switch(line) // upravime smer |
{ |
case S: //obe cidla na care |
// if (speedL<speedR) speedL=speedR; |
// else speedR=speedL; |
GO(L,F,speedL); GO(R,F,speedR) // jedeme rovne |
// if(rovinka<BRZDENI) rovinka++; //cara je rovne |
// dira=0; // videli jsme caru, proto neni dira |
continue; // vrat se zpet na cteni cidel |
case L: // cara je pod levym cidlem, trochu zatocime |
GO(L, F, speedL); GO(R,F, speedR); |
// if(rovinka<BRZDENI) rovinka++; //cara je celkem rovne |
// dira=0; |
continue; |
case R: // cara pod pravym cidlem |
GO(R, F, speedR); GO(L, F, speedL); |
// if(rovinka<BRZDENI) rovinka++; |
// dira=0; |
continue; |
default: // cara neni pod zadnym cidlem |
}*/ |
/*switch (last) // zatacka |
{ |
case L: // do leva |
BL;STOPR; //zabrzdeni leveho motoru |
for(;rovinka>0;rovinka--) //chvili pockej |
{ |
line = RSENSOR; // precteni cidel |
line |= LSENSOR << 1; // sestav informaci o care |
if(line!=0) goto sem; //kdyz najdes caru, zastav |
} |
STOPL; FR; // pokracuj v zataceni |
while(line==0) |
{ |
line = RSENSOR; // precteni cidel |
line |= LSENSOR << 1; // sestav informaci o care |
} |
movement=L; |
rovinka=0; //zataceli jsme, uz neni rovna cara |
break; |
case R: |
BR; STOPL; // zabrzdeni praveho motoru |
for(;rovinka>0;rovinka--) |
{ |
line = RSENSOR; // precteni cidel |
line |= LSENSOR << 1; // sestav informaci o care |
if(line!=0) goto sem; |
} |
STOPR; FL; |
while(line==0) |
{ |
line = RSENSOR; // precteni cidel |
line |= LSENSOR << 1; // sestav informaci o care |
} |
movement=R; |
rovinka=0; //zataceli jsme, uz neni rovna cara |
break; |
}*/ |
} // while(true) |
} |
/roboti/istrobot/merkur/PIC16F88/turn_R/verze/2 tank.c |
---|
0,0 → 1,331 |
#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 100 // po jakem case zataceni se detekuje dira |
#define INC_SPEED 1 // prirustek rychlosti v jednom kroku |
#define FW_POMALU 100 // trochu mimo caru vnitrni pas |
#define FW_ZATACKA 200 // rychlost vnejsiho kola pri zataceni |
#define FW_STREDNE 110 // trochu mimo caru vnejsi pas |
#define COUVANI 600 // 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 |
//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)) |
{ |
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(); |
sprintf(AXstring,"L: %U R: %U\0", ls, rs); // Convert DATA to String. |
SendPacket(&AXstring[0]); |
delay_ms(1000); |
}; |
} |
/////////////////////////////////////////////////////////////////////////////// |
void cikcak() |
{ |
int n; |
switch(movement) // podivej se na jednu stranu |
{ |
case L: |
FL;BR; |
movement=R; |
break; |
case R: |
FR;BL; |
movement=L; |
break; |
case S: |
FR;BL; |
movement=L; |
break; |
} |
while (0==(RSENSOR|LSENSOR)) |
{ |
if (n==50) //cara asi bude na druhe strane |
{ |
STOPR;STOPL; |
n=0; |
switch(movement) |
{ |
case L: |
FL;BR; |
movement=R; |
break; |
case R: |
FR;BL; |
movement=L; |
break; |
} |
} |
Delay_ms(5); |
n++; |
} |
STOPL;STOPR; // nasli jsme caru |
line=S; |
} |
/////////////////////////////////////////////////////////////////////////////// |
void objizdka() |
{ |
BL;BR;Delay_ms(300); |
STOPR;STOPL; |
beep(1000,1000); |
Delay_ms(500); |
beep(1000,1000); |
Delay_ms(1000); |
} |
/////////////////////////////////////////////////////////////////////////////// |
void prejeddiru() |
{ |
STOPL;STOPR; |
beep(800,500); |
Delay_ms(50); |
beep(800,500); |
switch (movement) //vrat se zpet na caru |
{ |
case L: |
BR;Delay_ms(COUVANI);STOPR; |
STOPL;STOPR; |
break; |
case R: |
BL;Delay_ms(COUVANI);STOPL; |
STOPL;STOPR; |
break; |
case S: |
BL; BR; Delay_ms(COUVANI); |
STOPL; STOPR; |
break; |
} |
FR;FL; Delay_ms(PRES_DIRU); // popojedem dopredu mozna tam bude cara |
STOPL; STOPR; |
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_16,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; |
switch (sensors) // zatacej podle toho, kde vidis caru |
{ |
case S: // rovne |
GO(L, F, speed); GO(R, F, speed); |
// FL; FR; // pokud se jede dlouho rovne, tak pridej |
dira=0; |
continue; |
case L: // trochu vlevo |
GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka); |
line=L; |
dira=0; |
continue; |
case R: // trochu vpravo |
GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka); |
line=R; |
dira=0; |
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; |
} |
else speed=255; |
/* if (dira==0) |
{ |
if (L==line) // kdyz jsou obe cidla mimo caru, zabrzdi vnitrni kolo |
{ |
BL; |
for(n=4000;n>0;n--) // Delay |
{ |
line = RSENSOR; // precteni cidel |
line |= LSENSOR << 1; // sestav informaci o care |
if(line!=0) break; |
} |
STOPL; |
} |
else |
{ |
BR; |
for(n=4000;n>0;n--) // Delay |
{ |
line = RSENSOR; // precteni cidel |
line |= LSENSOR << 1; // sestav informaci o care |
if(line!=0) break; |
} |
STOPR; |
} |
}*/ |
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) |
} |
/roboti/istrobot/merkur/PIC16F88/turn_R/verze/tank_funkcni.BAK |
---|
0,0 → 1,352 |
#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 |
unsigned int8 cihla; // urcuje za jak dlouho muze byt znova detekovana cihla |
// 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 100 // 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++; |
if (cihla>0) cihla--; |
} |
// 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(); |
sprintf(AXstring,"L: %U R: %U\0", ls, rs); // Convert DATA to String. |
SendPacket(&AXstring[0]); |
delay_ms(1000); |
}; |
} |
/////////////////////////////////////////////////////////////////////////////// |
void cikcak() |
{ |
unsigned int16 n; |
sem1: |
n=CIK_CAK; |
while (0==RSENSOR) // 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(); |
BR; FL; Delay_ms(215); // otoc se 70° do prava |
FR; FL; Delay_ms(600); // popojed rovne |
BL; Delay_ms(30); // otoc se 90° do leva |
STOPL; FR; Delay_ms(600); |
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;} |
Delay_ms(1); |
} |
BR; // otoc se 60° do prava |
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(); |
cihla=T_CIHLA; |
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 |
cihla=T_CIHLA; |
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) && (cihla==0)) 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) |
} |
/roboti/istrobot/merkur/PIC16F88/turn_R/verze/tank_funkcni.c |
---|
0,0 → 1,352 |
#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 |
unsigned int8 cihla; // urcuje za jak dlouho muze byt znova detekovana cihla |
// 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 100 // 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++; |
if (cihla>0) cihla--; |
} |
// 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(); |
sprintf(AXstring,"L: %U R: %U\0", ls, rs); // Convert DATA to String. |
SendPacket(&AXstring[0]); |
delay_ms(1000); |
}; |
} |
/////////////////////////////////////////////////////////////////////////////// |
void cikcak() |
{ |
unsigned int16 n; |
sem1: |
n=CIK_CAK; |
while (0==RSENSOR) // 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(); |
BR; FL; Delay_ms(215); // otoc se 70° do prava |
FR; FL; Delay_ms(600); // popojed rovne |
BL; Delay_ms(30); // otoc se 90° do leva |
STOPL; FR; Delay_ms(600); |
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;} |
Delay_ms(1); |
} |
BR; // otoc se 60° do prava |
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(); |
cihla=T_CIHLA; |
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 |
cihla=T_CIHLA; |
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) && (cihla==0)) 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) |
} |
/roboti/istrobot/merkur/PIC16F88/turn_R/verze/tank_funkcni_vecer.c |
---|
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(225); // 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(150); // 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;} |
Delay_ms(1); |
} |
BR; // otoc se 60° do prava |
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(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) |
} |