Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 250 → Rev 252

/roboti/istrobot/camerus/SW/876/camerus.c
7,9 → 7,10
#USE FAST_IO (C) // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
 
// Rychlostni konstanty
#define RR_CIHLA 30 // Rozumna rychlost pro objizdeni cihly
#define RR_CIHLA 25 // 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 TUHOS 100 // Jak dlouho se bude couvat po narazu na naraznik
#define ODODO_CIHLA 0xFFF
#define ODODO_TUNEL 0xFFF
#define ODODO_PRERUSENI 0xFFF
143,7 → 144,7
void LogLog(int8 flag, int16 gap)
{
int16 timer_pom;
 
timer_pom=get_timer1(); // Timer se musi vycist atomicky
bb_l[log]=make8(timer_pom,0); // Zaznam
bb_h[log]=make8(timer_pom,1);
158,7 → 159,7
last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano?
{
int8 n,i;
 
i=0;
for(n=0;n<=last_log;n++)
{
166,7 → 167,7
if(read_eeprom(i)==0xFF) odo_cihla=MAKE16(read_eeprom(i+2),read_eeprom(i+1));
if((read_eeprom(i)>0) && (read_eeprom(i)<0xFF)) odo_preruseni=MAKE16(read_eeprom(i+2),read_eeprom(i+1));
}
}
}
}
 
 
239,7 → 240,7
SetServo(CASAVR-CASMIN);
}
 
#include ".\diag.c"
#include ".\diag.c"
 
//---------------------------- INT --------------------------------
#int_EXT
308,7 → 309,8
setup_adc(ADC_CLOCK_INTERNAL);
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro mereni casu hrany W/B v radce
setup_timer_1(T1_EXTERNAL); // Cita pulzy z odometrie z praveho kola
setup_timer_2(T2_DIV_BY_16,255,1); // Casovac PWM motoru
//!!! setup_timer_2(T2_DIV_BY_16,255,1); // Casovac PWM motoru
setup_timer_2(T2_DIV_BY_4,255,1); // Casovac PWM motoru
setup_ccp1(CCP_PWM); // RC1 // PWM pro motory
setup_ccp2(CCP_PWM); // RC2
setup_comparator(NC_NC_NC_NC);
447,7 → 449,8
 
set_adc_channel(ZELENA); // --- Kroutitko pro vykon motoru ---
delay_ms(1);
rr=read_adc()>>2; // 0-63 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!
//!!! rr=read_adc()>>2; // 0-63 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!
rr=read_adc()>>3; // 0-31 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!
rrold=rr;
 
cas=CASAVR-CASMIN; // Inicializace promenych, aby neslo servo za roh
536,10 → 539,10
if(last_log_odo<get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu
{
LogLog(0,16); // Priznak tunelu; dalsi mereni nejdrive po ujeti 48 cm
rr=rrold; // Vjeli jsme do tunelu, znovu jed rychle
rr=rrold; // Vjeli jsme do tunelu, znovu jed rychle
}
};
 
//ODODO
ododo=get_timer1();
if((ododo>odo_preruseni)&&(ododo<(odo_preruseni+8))) rr=RR_PRERUSENI;
548,11 → 551,11
 
// 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); // rozsah 1 az 92 pro rr=0
// rozsah 1 az 154 pro rr=63
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 pro rr=0
//!!! pro zatuhle prevodovky
r1<<=1; // Rychlost je dvojnasobna
r2<<=1; // Rozsah 2 az 184 pro rr=0
 
if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle)) // Jizda
{
611,11 → 614,12
{
bum();
SaveLog(log-1); // Zapis Black Boxu do EEPROM
set_pwm1_duty(160); // pomalu vpred
set_pwm2_duty(160);
delay_ms(TUHOS); //!!! Zatuhle prevodovky
set_pwm1_duty(200); // pomalu vpred
set_pwm2_duty(200);
output_low(MOT_L);
output_low(MOT_R);
cas=CASAVR-CASMIN;
cas=CASAVR-CASMIN;
};
set_adc_channel(LMAX); // Levy UV sensor
for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
/roboti/istrobot/camerus/SW/876/objizdka_L.c
20,7 → 20,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(200);
set_pwm2_duty(255);
output_high(MOT_L);
output_low(MOT_R);
odocounter=get_timer1();
32,12 → 32,13
{
bum();
SaveLog(log-1); // Zapis Black Boxu do EEPROM
delay_ms(TUHOS); //!!! Zatuhle prevodovky
brzda();
goto cihla; // Znovu zacni cihlu objizdet
};
 
set_pwm1_duty(0);
set_pwm2_duty(220); // !!! mozna prilis maly vykon pro rozjezd pro zatuhlou prevodovku
set_pwm2_duty(255); // !!! mozna prilis maly vykon pro rozjezd pro zatuhlou prevodovku
output_high(MOT_L); // leve kolo reverz
output_low(MOT_R); // prave kolo vpred
if(get_timer1()>(odocounter+5)) // konec zatacky?
61,6 → 62,7
{
bum();
SaveLog(log-1); // Zapis Black Boxu do EEPROM
delay_ms(TUHOS); //!!! Zatuhle prevodovky
set_pwm1_duty(160); // vpred
set_pwm2_duty(160);
output_low(MOT_L);
93,6 → 95,10
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);
 
//!!! pro zatuhle prevodovky
r1<<=1; // Rychlost je dvojnasobna
r2<<=1; // Rozsah 2 az 184 pro rr=0
 
set_pwm1_duty(r1); // Nastav rychlost motoru
set_pwm2_duty(r2);
 
141,8 → 147,8
};
disp(0xC3);
 
set_pwm1_duty(20);
set_pwm2_duty(200);
set_pwm1_duty(0); //!!! pred zatuhlejma prevodovkama tam bylo 20 a 200
set_pwm2_duty(255);
output_high(MOT_L);
output_low(MOT_R);
delay_us(40);