Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 237 → Rev 238

/roboti/istrobot/camerus/SW/876/camerus.PJT
37,6 → 37,21
4=C:\dokumenty\svn\Kaklik\roboti\istrobot\camerus\SW\876\objizdka_L.c
5=
6=
[debugperif]
selected=Analog/Digital Conv
[debugram]
autoread=1
[debugeedata]
autoread=1
[debugbreak]
count=0
[pcwdebug]
watchcol0=75
[debugwatch]
count=0
[debugexpr]
expr=
sideeffects=0
[Units]
Count=1
1=C:\dokumenty\svn\Kaklik\roboti\istrobot\camerus\SW\876\camerus.c (main)
/roboti/istrobot/camerus/SW/876/camerus.c
7,8 → 7,8
#USE FAST_IO (C) // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
 
// Rychlostni konstanty
#define RR_CIHLA 20 // Rozumna rychlost pro objizdeni cihly
#define RR_PRERUSENI 20 // Rozumna rychlost pro priblizeni se k preruseni
#define RR_CIHLA 24 // Rozumna rychlost pro objizdeni cihly
#define RR_PRERUSENI 18 // Rozumna rychlost pro priblizeni se k preruseni
#define BRZDNA_DRAHA 8 // Jak daleko pred problemem se zacne brzdit
 
// Adresy IIC periferii
190,6 → 190,17
output_low(SERVO);
}
 
// Couvni po narazu na naraznik
inline void bum()
{
set_pwm1_duty(0); // couvni, rovne dozadu
set_pwm2_duty(0);
output_high(MOT_L);
output_high(MOT_R);
disp(0xA5);
SetServo(CASAVR-CASMIN);
}
 
//---------------------------- INT --------------------------------
#int_EXT
EXT_isr() // Preruseni od prekazky
216,10 → 227,14
return;
};
 
timer_pom=get_timer1(); // Obsah Timeru1 se musi nacist atomicky
bb_l[log]=make8(timer_pom,0); // Zaznam do logu
bb_h[log]=make8(timer_pom,1)|0x80; // Cihla ma nastaven nejvyssi bit
if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam
rr=rrold; // Po cihle se pojede opet Rozumnou Rychlosti
if(stav!=cihla)
{
timer_pom=get_timer1(); // Obsah Timeru1 se musi nacist atomicky
bb_l[log]=make8(timer_pom,0); // Zaznam do logu
bb_h[log]=make8(timer_pom,1)|0x80; // Cihla ma nastaven nejvyssi bit
if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam
};
//!!! Vyzkouset, ze nepreteka LOG
 
//!!! if(stav==cihla) while(true); // Zastav na furt, konec drahy
226,7 → 241,7
// if(stav==cihla) return; // Po druhe nic neobjizdej
// Pozor na rozjezd
 
if(stav==jizda) // Objed cihlu
if((stav==jizda)||(stav==cihla)) // Objed cihlu
{
#include ".\objizdka_L.c"
};
258,6 → 273,23
output_low(MOT_L); // Nastav smer vpred
output_low(MOT_R);
 
if(BUMPER) // Kdyz nekdo na zacatku drzi naraznik, vymaz log
{
int8 n;
for(n=0;n<255;n++) write_eeprom(n,0xFF);
bb_l[0]=0; // Zapis na pozici 0 vzdalenost 0
bb_h[0]=0;
SaveLog(0); // Zapis do EEPROM pocet zaznamu 0, tedy jeden zaznam
while(true)
{
disp(0x55); // Blikni pro potvrzeni
delay_ms(200);
disp(0xAA);
delay_ms(200);
}
}
 
output_low(IRTX); // Zapni LED na detekci prekazky
 
NightRider(1); // Aby se poznalo, ze byl RESET
409,17 → 441,6
log=0; // Zacatek logu v cerne skrince
last_log_odo=0; // Posledni zaznam odometrie do logu
 
if(BUMPER) // Kdyz nekdo na zacatku drzi naraznik, vymaz log
{
bb_l[0]=0; // Zapis na pozici 0 vzdalenost 0
bb_h[0]=0;
SaveLog(0); // Zapis do EEPROM pocet zaznamu 0, tedy jeden zaznam
disp(0x55); // Blikni pro potvrzeni
delay_ms(200);
disp(0xAA);
delay_ms(200);
}
 
last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano?
bb=MAKE16(read_eeprom(1) & 0x7F,read_eeprom(0)); // Precti prvni zaznam
 
428,7 → 449,11
{
int8 pom;
int8 n;
 
int8 gap;
int16 ododo;
gap=0; // Vynuluj pocitadlo preruseni
next_snap:
 
pom=0;
464,16 → 489,28
 
if(pom==0) // Kamera nevidi caru, poznamenej to do logu
{
if(last_log_odo>get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu
if((cas>(CASMIN+30))&&(cas<(CASMAX-30))) // Nebyla minule cara moc u kraje?
if(last_log_odo<get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu
{
timer_pom=get_timer1(); // Timer se musi poznamenat atomicky
bb_l[log]=make8(timer_pom,0); // Zaznam
bb_h[log]=make8(timer_pom,1);
last_log_odo=timer_pom+8; // Dalsi mereni nejdrive po ujeti 24 cm
if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam
gap++;
if(gap>=3) // Trva preruseni alespon 3 snimky?
{
timer_pom=get_timer1(); // Timer se musi poznamenat atomicky
bb_l[log]=make8(timer_pom,0); // Zaznam
bb_h[log]=make8(timer_pom,1);
last_log_odo=timer_pom+8; // Dalsi mereni nejdrive po ujeti 24 cm
if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam
rr=rrold; // Preruseni skoncilo, vrat Rozumnou Rychlost
}
}
}
else gap=0;
 
//ODODO
ododo=get_timer1();
if((ododo>0x1C)&&(ododo<0x38)) rr=RR_PRERUSENI;
if((ododo>0x4F)&&(ododo<0x68)) rr=RR_PRERUSENI;
// Elektronicky diferencial
if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;}; // pro rizeni rychlosti motoru
487,13 → 524,6
// r1<<=1; // Rychlost je dvojnasobna
// r2<<=1; // Rozsah 2 az 184
 
/* Nerozumna rychlost po cihle
if ((stav==cihla)&&(get_timer1()>(odocounter+5))) // Snizime rychlost po ujeti
{
rr=rrold;
stav=pocihle;
};
*/
if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle)) // Jizda
{
set_pwm1_duty(r1);
549,12 → 579,7
{
if(BUMPER) // Sakra, do neceho jsme narazili a nevideli jsme to!
{
set_pwm1_duty(0); // couvni, rovne dozadu
set_pwm2_duty(0);
output_high(MOT_L);
output_high(MOT_R);
disp(0x5A);
SetServo(CASAVR-CASMIN);
bum();
SaveLog(log-1); // Zapis Black Boxu do EEPROM
output_low(MOT_L); // Vypni motory
output_low(MOT_R);
/roboti/istrobot/camerus/SW/876/objizdka_L.c
11,11 → 11,12
okolo_cihly ridic; // V jakem jsme stavu objizdeni cihly
int8 vzdalenost;
 
stav=cihla; // Dalsi prekazku uz nezaznamenavej (je to s velkou pravdepodobnosti cil)
touch=0; // Indikator detekce cary pri objizdeni
 
cihla:
 
rr=20; //!!! Rozumna rychlost pro objizdeni cihly (bylo by lepsi rychlost zvysovat) a pri detekci pohybu zase snizit
rr=RR_CIHLA; //!!! Rozumna rychlost pro objizdeni cihly (bylo by lepsi rychlost zvysovat) a pri detekci pohybu zase snizit
disp(0x99);
set_pwm1_duty(0); // zabrzdi levym kolem, prave vpred
set_pwm2_duty(150);
26,6 → 27,13
{
cas=CASMIN-5; // jeste vic nez hodne do leva
 
if(BUMPER) // Narazili jsme do cihly, musime couvnout!
{
bum();
SaveLog(log-1); // Zapis Black Boxu do EEPROM
goto cihla; // Znovu zacni cihlu objizdet
};
 
set_pwm1_duty(0);
set_pwm2_duty(200);
output_high(MOT_L); // leve kolo reverz
48,12 → 56,7
{
if(BUMPER) // Narazili jsme do cihly, musime couvnout!
{
set_pwm1_duty(0); // couvni, rovne dozadu
set_pwm2_duty(0);
output_high(MOT_L);
output_high(MOT_R);
disp(0xA5);
SetServo(CASAVR-CASMIN);
bum();
SaveLog(log-1); // Zapis Black Boxu do EEPROM
goto cihla; // Znovu zacni cihlu objizdet
// Pozor! Pamatuje se, jestli jsme uz neprejeli caru!