Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 238 → Rev 239

/roboti/istrobot/camerus/SW/876/camerus.c
7,9 → 7,11
#USE FAST_IO (C) // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
 
// Rychlostni konstanty
#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
#define RR_CIHLA 30 // Rozumna rychlost pro objizdeni cihly
#define RR_PRERUSENI 20 // Rozumna rychlost pro priblizeni se k preruseni
#define BRZDNA_DRAHA 0x20 // Jak daleko pred problemem se zacne brzdit
#define ODODO1 0x37
#define ODODO2 0x5E
 
// Adresy IIC periferii
#define CAMERA_ADR 0xC0
44,8 → 46,8
#define CASAVR ((CASMAX+CASMIN) / 2)
 
#define EEMAX 255 // Konec EEPROM
#define MAXLOG 0x30 // Maximalni pocet zaznamu v logu
#if MAXLOG>(EEMAX/2-1)
#define MAXLOG 0x10 // Maximalni pocet zaznamu v logu
#if MAXLOG>(EEMAX/3)
#error Prekrocena velikost EEPROM
#endif
 
72,11 → 74,13
int16 last_log; // Cislo posledniho zaznamu v logu v EEPROM
int8 bb_h[MAXLOG]; // Cerna skrinka MSB
int8 bb_l[MAXLOG]; // Cerna skrinka LSB
int8 bb_f[MAXLOG]; // Cerna skrinka priznak (typ zaznamu)
int8 log; // Pocitadlo pro cernou skrinku
int16 bb; // Posledni nactena udalost z Black Boxu
int16 timer_pom; // Pomocna promenna pro atomicke nacteni hodnoty timeru1
int8 rr; // Promenna na ulozeni Rozumne rychlost
int8 rrold;
int16 odo1, odo2; // Problemy na trati
 
// Zobrazeni jednoho byte na modulu LEDbar
inline void disp(int8 x)
122,12 → 126,15
// Zaznam LOGu do EEPROM
void SaveLog(int8 log)
{
int8 n;
int8 n,i;
 
for(n=0;n<=(log*2);n+=2) // Ulozeni Black Boxu do EEPROM
i=0;
for(n=0;n<=(log*3);n+=3) // Ulozeni Black Boxu do EEPROM
{
write_eeprom(n,bb_h[n/2]);
write_eeprom(n+1,bb_l[n/2]);
write_eeprom(n,bb_f[i]);
write_eeprom(n+1,bb_h[i]);
write_eeprom(n+2,bb_l[i]);
i++;
};
write_eeprom(EEMAX,log); // Zapis poctu zaznamu na konec EEPROM
}
164,7 → 171,7
{
int8 n;
 
for(n=0; n<14; n++)
for(n=0; n<10; n++)
{
output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo
delay_us(1000);
232,7 → 239,8
{
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
bb_h[log]=make8(timer_pom,1);
bb_f[log]=0xFF; // Cihla
if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam
};
//!!! Vyzkouset, ze nepreteka LOG
276,8 → 284,8
if(BUMPER) // Kdyz nekdo na zacatku drzi naraznik, vymaz log
{
int8 n;
for(n=0;n<255;n++) write_eeprom(n,0xFF);
 
for(n=0;n<255;n++) write_eeprom(n,0);
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
431,7 → 439,7
 
set_adc_channel(ZELENA); // --- Kroutitko pro vykon motoru ---
delay_ms(1);
rr=read_adc()>>2; // 0-31 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni !!!
rr=read_adc()>>2; // 0-63 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!
rrold=rr;
 
cas=CASAVR-CASMIN; // Inicializace promenych, aby neslo servo za roh
444,6 → 452,9
last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano?
bb=MAKE16(read_eeprom(1) & 0x7F,read_eeprom(0)); // Precti prvni zaznam
 
odo1=ODODO1-BRZDNA_DRAHA;
odo2=ODODO2-BRZDNA_DRAHA;
// ........................... Hlavni smycka ................................
while(true)
{
451,13 → 462,13
int8 n;
int8 gap;
int16 ododo;
 
gap=0; // Vynuluj pocitadlo preruseni
 
next_snap:
 
pom=0;
disable_interrupts(GLOBAL); //----------------------- Critical
disable_interrupts(GLOBAL); //----------------------- Critical Section
while(input(HREF)); // Preskoc 1. radku
while(!input(HREF)); // Cekej nez se zacnou posilat pixely z 2. radky
set_timer0(0); // Vynuluj pocitadlo casu
487,6 → 498,13
delay_us(cas);
output_low(SERVO);
 
// Elektronicky diferencial 1. cast
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
if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
 
enable_interrupts(GLOBAL); //----------------------- End Critical Section
 
if(pom==0) // Kamera nevidi caru, poznamenej to do logu
{
if((cas>(CASMIN+30))&&(cas<(CASMAX-30))) // Nebyla minule cara moc u kraje?
493,33 → 511,32
if(last_log_odo<get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu
{
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;
else
{
if(gap>=2) // Trva preruseni cary alespon 2 snimky?
{
timer_pom=get_timer1(); // Timer se musi vycist atomicky
bb_l[log]=make8(timer_pom,0); // Zaznam
bb_h[log]=make8(timer_pom,1);
bb_f[log]=gap;
last_log_odo=timer_pom+8; // Dalsi mereni nejdrive po ujeti 24 cm
if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam
rr=rrold; // Preruseni cary skoncilo, znovu jed Rozumnou Rychlosti
}
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
if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
if((ododo>odo1)&&(ododo<(odo1+8))) rr=RR_PRERUSENI;
if((ododo>odo2)&&(ododo<(odo2+8))) rr=RR_PRERUSENI;
 
enable_interrupts(GLOBAL); //----------------------- Critical
 
// Elektronicky diferencial 2. cast
if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN); // Neco jako nasobeni
if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN); // rozsah 1 az 92 pro rr=0
// rozsah 1 az 154 pro rr=63
 
// r1<<=1; // Rychlost je dvojnasobna
// r2<<=1; // Rozsah 2 az 184
581,7 → 598,9
{
bum();
SaveLog(log-1); // Zapis Black Boxu do EEPROM
output_low(MOT_L); // Vypni motory
set_pwm1_duty(140); // pomalu vpred
set_pwm2_duty(140);
output_low(MOT_L);
output_low(MOT_R);
};
set_adc_channel(LMAX); // Levy UV sensor
/roboti/istrobot/camerus/SW/876/objizdka_L.c
19,7 → 19,7
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);
set_pwm2_duty(200);
output_high(MOT_L);
output_low(MOT_R);
odocounter=get_timer1();
31,6 → 31,7
{
bum();
SaveLog(log-1); // Zapis Black Boxu do EEPROM
brzda();
goto cihla; // Znovu zacni cihlu objizdet
};
 
58,8 → 59,10
{
bum();
SaveLog(log-1); // Zapis Black Boxu do EEPROM
goto cihla; // Znovu zacni cihlu objizdet
// Pozor! Pamatuje se, jestli jsme uz neprejeli caru!
set_pwm1_duty(140); // vpred
set_pwm2_duty(140);
output_low(MOT_L);
output_low(MOT_R);
};
 
if(IRRX) // hrozi celni srazka s cihlou v prubehu objizdeni