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); |
/* |