/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) |
} |