Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 235 → Rev 236

/roboti/istrobot/camerus/SW/876/camerus.c
6,9 → 6,14
 
#USE FAST_IO (C) // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
 
#define CAMERA_adr 0xC0
#define SONAR_adr 0xE0
// Rychlostni konstanty
#define RR_CIHLA 20 // Rozumna rychlost pro objizdeni cihly
#define RR_PRERUSENI 20 // Rozumna rychlost pro priblizeni se k preruseni
#define BRZDNA_DRAHA 8 // Jak daleko pred problemem se zacne brzdit
 
#define CAMERA_ADR 0xC0
#define SONAR_ADR 0xE0
 
// A/D vstupy
#define RMAX 4 // AN4/RA5 - leve cidlo na vyjeti z cary
#define LMAX 3 // AN3/RA3 - prave cidlo na vyjeti z cary
28,14 → 33,21
#define CP PIN_B1 // K modulu LEDbar hodiny
//#define ODO PIN_C0 // Ze snimace z odometrie z praveho kola na TIMER1
// Jeden impuls je 31,25mm
#define IRRX PIN_B0 // Vstup INT, generuje preruseni pri prekazce
#define IRRX !input(PIN_B0) // Vstup INT, generuje preruseni pri prekazce
#define IRTX PIN_B3 // Modulovani vysilaci IR LED na detekci prekazky
#define PROXIMITY PIN_C7 // Cidlo kratkeho dosahu na cihlu
#define BUMPER !input(PIN_A4) // Naraznik
 
#define CASMIN 6 // Rozsah radku snimace
#define CASMAX 192
#define CASAVR ((CASMAX+CASMIN) / 2)
 
#define EEMAX 255 // Konec EEPROM
#define MAXLOG 0x30 // Maximalni pocet zaznamu v logu
#if MAXLOG>(EEMAX/2-1)
#error Prekrocena velikost EEPROM
#endif
 
#define OFFSETO 0x9F //0x9F // Vystredeni serva pro objeti prekazky
 
#define THR 90 // Threshold pro UV cidla na caru
54,8 → 66,15
stavy stav; // Kde jsme na trati
int8 cas; // Cas hrany bila/cerna v radce
int8 stred; // Vystredeni kolecka
int16 odocounter; // Zaznamenani aktualniho stavu pocitadla odometrie
int8 rr; // Promenna na ulozeni Rozumne rychlost
int16 odocounter; // Zaznamenani aktualniho stavu pocitadla odometrie
int16 last_log_odo; // Posledni stav odometrie poznamenany do logu
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 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;
 
// Zobrazeni jednoho byte na modulu LEDbar
65,7 → 84,7
 
for(n=0;n<=7;n++)
{
if (x & 1 == 1) output_low(DATA); else output_high(DATA);
if (bit_test(x,0)) output_low(DATA); else output_high(DATA);
output_high(CP);
x>>=1;
output_low(CP);
99,6 → 118,19
disp(i);
}
 
// Zaznam LOGu do EEPROM
void SaveLog(int8 log)
{
int8 n;
 
for(n=0;n<=(log*2);n+=2) // Ulozeni Black Boxu do EEPROM
{
write_eeprom(n,bb_h[n/2]);
write_eeprom(n+1,bb_l[n/2]);
};
write_eeprom(EEMAX,log); // Zapis poctu zaznamu na konec EEPROM
}
 
// Brzdeni motorama stridou 1:1
void brzda()
{
168,7 → 200,7
// Ujistime se, ze prijaty signal je z naseho IR vysilace
output_high(IRTX); // Vypni LED na detekci prekazky
delay_ms(2);
if (!input(IRRX)) // stale nas signal?
if (IRRX) // stale nas signal?
{
output_low(MOT_L); // je odraz -> vpred
output_low(MOT_R);
176,7 → 208,7
};
output_low(IRTX); // Zapni LED na detekci prekazky
delay_ms(10);
if (input(IRRX)) // stale nas signal?
if (!IRRX) // stale nas signal?
{
output_low(MOT_L); // neni odraz -> vpred
output_low(MOT_R);
183,6 → 215,12
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
//!!! Vyzkouset, ze nepreteka LOG
 
//!!! if(stav==cihla) while(true); // Zastav na furt, konec drahy
// if(stav==cihla) return; // Po druhe nic neobjizdej
// Pozor na rozjezd
190,7 → 228,8
if(stav==jizda) // Objed cihlu
{
#include ".\objizdka_L.c"
}
};
last_log_odo=get_timer1()+16; // Pul metru po cihle nezaznamenavej do LOGu
}
 
 
225,12 → 264,12
 
//... Nastaveni sonaru ...
i2c_start();
i2c_write(SONAR_adr);
i2c_write(SONAR_ADR);
i2c_write(0x02); // dosah
i2c_write(0x03); // n*43mm
i2c_stop();
i2c_start();
i2c_write(SONAR_adr);
i2c_write(SONAR_ADR);
i2c_write(0x01); // zesileni
i2c_write(0x01); // male, pro eliminaci echa z minuleho mereni
i2c_stop();
259,13 → 298,13
 
//... Nastaveni kamery ...
i2c_start(); // Soft RESET kamery
i2c_write(CAMERA_adr); // Pro single slave musi mit vsechny zapisy adresu C0h
i2c_write(CAMERA_ADR); // Pro single slave musi mit vsechny zapisy adresu C0h
i2c_write(0x12); // Adresa registru COMH
i2c_write(0x80 | 0x24); // Zapis ridiciho slova
i2c_stop();
 
i2c_start(); // BW
i2c_write(CAMERA_adr);
i2c_write(CAMERA_ADR);
i2c_write(0x28);
i2c_write(0b01000001);
i2c_stop();
285,19 → 324,19
*/
 
i2c_start(); // Fame Rate
i2c_write(CAMERA_adr);
i2c_write(CAMERA_ADR);
i2c_write(0x2B);
i2c_write(0x00); // cca 17ms (puvodni hodnota 5Eh = 20ms)
i2c_stop();
 
i2c_start(); // VSTRT
i2c_write(CAMERA_adr);
i2c_write(CAMERA_ADR);
i2c_write(0x19);
i2c_write(118); // prostredni radka
i2c_stop();
 
i2c_start(); // VEND
i2c_write(CAMERA_adr);
i2c_write(CAMERA_ADR);
i2c_write(0x1A);
i2c_write(118);
i2c_stop();
308,7 → 347,7
int8 t1,t2;
 
i2c_start(); // Brightness, zacni od uplne tmy
i2c_write(CAMERA_adr);
i2c_write(CAMERA_ADR);
i2c_write(0x06);
i2c_write(0); // 80h default
i2c_stop();
317,7 → 356,7
for(offset=0x04;offset<(255-0x04);offset+=0x04) // Zacni od jasu 10h
{
i2c_start(); // Brightness
i2c_write(CAMERA_adr);
i2c_write(CAMERA_ADR);
i2c_write(0x06);
i2c_write(offset); // 80h default
i2c_stop();
366,7 → 405,23
// a aby se to rozjelo jeste dneska
stav=start; // Jsme na startu
set_timer1(0); // Vynuluj citac odometrie
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
 
// ........................... Hlavni smycka ................................
while(true)
{
406,6 → 461,18
delay_us(cas);
output_low(SERVO);
 
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
{
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
}
}
 
// 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
477,8 → 544,20
for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
disp(pom);
 
while(true) // Ve zbytku casu snimku cti krajni UV senzory
while(true) // Ve zbytku casu snimku cti krajni UV senzory a naraznik
{
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);
SaveLog(log-1); // Zapis Black Boxu do EEPROM
output_low(MOT_L); // Vypni motory
output_low(MOT_R);
};
set_adc_channel(LMAX); // Levy UV sensor
for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
if(read_adc()<THR) cas=CASMIN;
/roboti/istrobot/camerus/SW/876/objizdka_L.c
58,7 → 58,7
goto cihla; // Znovu zacni cihlu objizdet
// Pozor! Pamatuje se, jestli jsme uz neprejeli caru!
};
 
if(IRRX) // hrozi celni srazka s cihlou v prubehu objizdeni
{
cas=CASMIN;
88,7 → 88,7
SetServoQ(cas);
 
i2c_start(); // Sonar Ping
i2c_write(0xE0);
i2c_write(SONAR_ADR);
i2c_write(0x0);
i2c_write(0x52); // mereni v us
i2c_stop();
104,11 → 104,11
};
 
i2c_start(); // Odraz ze sonaru
i2c_write(0xE0);
i2c_write(SONAR_ADR);
i2c_write(0x3);
i2c_stop();
i2c_start();
i2c_write(0xE1);
i2c_write(SONAR_ADR+1);
vzdalenost=i2c_read(0);
i2c_stop();