Subversion Repositories svnkaklik

Compare Revisions

Regard whitespace Rev 225 → Rev 226

/roboti/istrobot/camerus/SW/876/camerus.c
35,8 → 35,7
 
#define OFFSETO 0x9F //0x9F // Vystredeni serva pro objeti prekazky
 
#define THR_H 128
#define THR_L 90
#define THR 90 // Threshold pro UV cidla na caru
 
#byte INTCON = 0x0B // Interrupt configuration register
#bit GIE = INTCON.7
155,6 → 154,7
output_low(SERVO);
}
 
//---------------------------- INT --------------------------------
#int_EXT
EXT_isr() // Preruseni od prekazky
{
172,7 → 172,7
return;
};
output_low(IRTX); // Zapni LED na detekci prekazky
delay_ms(5);
delay_ms(10);
if (input(IRRX)) // stale nas signal?
{
output_low(MOT_L); // neni odraz -> vpred
426,10 → 426,10
{
set_adc_channel(LMAX); // Levy UV sensor
delay_us(40);
if(read_adc()<THR_L) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo
if(read_adc()<THR) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo
set_adc_channel(RMAX); // Pravy UV sensor
delay_us(40);
if(read_adc()<THR_L) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo
if(read_adc()<THR) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo
cas=CASAVR-CASMIN; // Cara je rovne
};
stav=rozjezd;
444,10 → 444,10
{
set_adc_channel(LMAX); // Levy UV sensor
for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
if(read_adc()<THR_L) cas=CASMIN;
if(read_adc()<THR) cas=CASMIN;
set_adc_channel(RMAX); // Pravy UV sensor
for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
if(read_adc()<THR_L) cas=CASMAX;
if(read_adc()<THR) cas=CASMAX;
};
}
}
/roboti/istrobot/camerus/SW/876/objizdka_L.c
1,39 → 1,53
// **** Objeti cihly vlevo **** LLLL
 
#define L_TOUCH 1 // Cara vlevo
#define R_TOUCH 2 // Cata vpravo
#define B_TOUCH 3 // Both
 
int8 n;
int8 r1,r2,rr;
int1 flag;
int8 touch;
enum okolo_cihly {pred_carou,na_care,po_care};
okolo_cihly ridic; // V jakem jsme stavu objizdeni cihly
 
SetServo(CASMIN-5); // max. doleva, zatoc kolmo na caru
rr=20;
 
disp(0x99);
odocounter=get_timer1();
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
output_low(MOT_R); // prave kolo vpred
if(get_timer1()>(odocounter+5)) // konec zatacky?
{
disp(0x66);
cas=CASAVR-CASMIN; // rovne
break;
}
SetServoQ(cas);
delay_ms(18);
};
 
touch=0;
ridic=pred_carou;
output_low(MOT_L); // vpred
output_low(MOT_R);
disp(0x81);
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
 
set_pwm1_duty(80); // zpomal
set_pwm2_duty(80);
set_adc_channel(LMAX);
disp(0x82);
cas=CASAVR-CASMIN; // rovne
odocounter=get_timer1();
flag=1;
rr=20;
while(true)
{
if(!input(PROXIMITY))
 
if(!input(PROXIMITY)) // Udrzovani konstantni vzdalenosti od cihly
{
if(cas>(CASMIN+10)) cas-=10;
if(cas>(CASMIN+20)) cas-=20;
}
else
{
if(cas<(CASMAX-10)) cas+=10;
if(cas<(CASMAX-20)) cas+=20;
};
 
SetServoQ(cas);
 
// 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
42,41 → 56,31
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);
set_pwm1_duty(r1); // Nastav rychlost motoru
set_pwm2_duty(r2);
 
SetServoQ(cas);
 
for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
{
/* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
if(get_timer1()>(odocounter+5)) // Prodleva, nez se zacne detekovat cara
{
if(flag==1) {disp(0x83); flag=0;};
if(read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) break;};}; // Neni to jen skvira nebo spina?
};
*/
set_adc_channel(LMAX);
delay_us(100);
if(read_adc()<THR_L) {set_adc_channel(RMAX); delay_us(40); disp(0x13); goto cara1;};
if(read_adc()<THR) touch|=L_TOUCH;
set_adc_channel(RMAX);
delay_us(100);
if(read_adc()<THR_L) {set_adc_channel(LMAX); delay_us(40); disp(0x23); goto cara1;};
if(read_adc()<THR) touch|=R_TOUCH;
};
if(touch==L_TOUCH) disp(0xC0);
if(touch==R_TOUCH) disp(0x03);
if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care;
if((ridic==na_care)&&(touch==0)) break;
if(ridic==na_care) touch=0;
};
disp(0xC3);
 
cara1:
 
//odocounter=get_timer1(); // Poznamenej aktualni stav odometrie
//!!! SetServo(CASAVR); // Primy smer !!!!!!!!!!!! to se nestiha
//set_pwm1_duty(140); // zrychli
//set_pwm2_duty(140);
while(read_adc()>THR_L); // I druhym senzorem dojed k care
disp(0x84);
 
set_adc_channel(LMAX);
// SetServo(CASMIN); // max. doleva L
set_pwm1_duty(20);
set_pwm1_duty(0);
set_pwm2_duty(170);
output_high(MOT_L); // levym kolem brzdi
output_low(MOT_L);
output_low(MOT_R);
delay_us(40);
odocounter=get_timer1(); // Poznamenej aktualni stav odometrie
84,33 → 88,34
{
for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
{
if(read_adc()<THR_L) // Dotkli jsme se levym senzorem
set_adc_channel(LMAX); // Levy UV sensor
delay_us(100);
if(read_adc()<THR) // Dotkli jsme se levym senzorem
{
while(read_adc()<THR_L); // Prejedeme caru
disp(0xE0);
cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
goto cara2;
goto cara;
};
set_adc_channel(RMAX); // Pravy UV sensor
delay_us(100);
if((get_timer1()>=(odocounter+2)) && (read_adc()<THR_L)) // Pravym senzorem nesmime caru prejet!
if((get_timer1()>=(odocounter+2)) && (read_adc()<THR)) // Pravym senzorem nesmime caru prejet!
{
disp(0x07);
cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
goto cara2;
goto cara;
};
set_adc_channel(LMAX); // Levy UV sensor
delay_us(100);
}
SetServoQ(CASMIN); // max. doleva L
}
 
cara2:
 
cara:
;
/*
set_pwm1_duty(0); // vypni motory
set_pwm2_duty(0);
 
SetServo(CASAVR-CASMIN); // doprostred
 
disp(0x85);
set_pwm1_duty(255); // max. vpred
set_pwm2_duty(255);
output_low(MOT_L);
120,8 → 125,9
{
set_adc_channel(LMAX); // Levy UV sensor
delay_us(40);
if(read_adc()<THR_L) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo
if(read_adc()<THR) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo
set_adc_channel(RMAX); // Pravy UV sensor
delay_us(40);
if(read_adc()<THR_L) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo
if(read_adc()<THR) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo
};
*/