Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 228 → Rev 229

/roboti/istrobot/camerus/SW/876/camerus.c
221,16 → 221,38
// taky se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
 
//... Nastaveni sonaru ...
i2c_start();
i2c_start();
i2c_write(0xE0);
i2c_write(0x02); // dosah
i2c_write(0x02); // 86mm
i2c_write(0x03); // n*43mm
i2c_stop();
i2c_start();
i2c_start();
i2c_write(0xE0);
i2c_write(0x01); // zesileni
i2c_write(0x00);
i2c_write(0x01); // male, pro eliminaci echa z minuleho mereni
i2c_stop();
// pro ladeni sonaru
/*
while(true)
{
i2c_start(); // Sonar Ping
i2c_write(0xE0);
i2c_write(0x0);
i2c_write(0x51); // 50 mereni v palcich, 51 mereni v cm, 52 v us
i2c_stop();
delay_ms(100);
i2c_start(); // Odraz ze sonaru
i2c_write(0xE0);
i2c_write(0x3);
i2c_stop();
i2c_start();
i2c_write(0xE1);
cas=i2c_read(0);
i2c_stop();
disp(cas);
}
*/
 
//... Nastaveni kamery ...
i2c_start(); // Soft RESET kamery
/roboti/istrobot/camerus/SW/876/objizdka_L.c
23,7 → 23,7
while(true) // Na zacatku se vyhni cihle, zatoc co muzes
{
cas=CASMIN-5; // jeste vic nez hodne do leva
 
set_pwm1_duty(0);
set_pwm2_duty(200);
output_high(MOT_L); // leve kolo reverz
36,7 → 36,7
SetServoQ(cas);
delay_ms(18);
};
 
//------ Objeti cihly v konstantni vzdalenosti ------
touch=0;
ridic=pred_carou;
46,7 → 46,7
while(true)
{
// if(!input(IRRX)) goto cihla;
 
if((vzdalenost!=0)||!input(PROXIMITY)) // Udrzovani konstantni vzdalenosti od cihly
{
if(cas>(CASMIN+20)) cas-=20;
55,24 → 55,24
{
if(cas<(CASMAX-20)) cas+=20;
};
 
// 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 (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN); // Neco jako nasobeni
if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
 
set_pwm1_duty(r1); // Nastav rychlost motoru
set_pwm2_duty(r2);
 
SetServoQ(cas);
 
i2c_start(); // Sonar Ping
i2c_write(0xE0);
i2c_write(0x0);
i2c_write(0x51);
i2c_write(0x0);
i2c_write(0x52); // mereni v us
i2c_stop();
 
for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
87,11 → 87,11
 
i2c_start(); // Odraz ze sonaru
i2c_write(0xE0);
i2c_write(0x3);
i2c_write(0x3);
i2c_stop();
i2c_start();
i2c_start();
i2c_write(0xE1);
vzdalenost=i2c_read(0);
vzdalenost=i2c_read(0);
i2c_stop();
 
if(touch==L_TOUCH) disp(0xC0);
101,7 → 101,7
if(ridic==na_care) touch=0;
};
disp(0xC3);
 
set_pwm1_duty(20);
set_pwm2_duty(200);
output_high(MOT_L);
131,9 → 131,9
}
SetServoQ(CASMIN-5); // max. max. doleva L
}
 
cara:
 
output_low(MOT_L); // oba motory vpred
output_low(MOT_R);
/*