Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 181 → Rev 184

/roboti/istrobot/camerus/SW/876/camerus.c
18,11 → 18,184
#define DATA PIN_B2 // K modulu LEDbar data
#define CP PIN_B1 // K modulu LEDbar hodiny
#define ODO PIN_A4 // Ze snimace z odometrie z praveho kola
//#define CIHLA PIN_B0 // Vstup INT, generuje preruseni pri prekazce
 
#define CASMIN 6 // Rozsah radku snimace
#define CASMAX 192
#define CASAVR ((CASMAX+CASMIN) / 2)
 
#byte INTCON = 0x0B // Interrupt configuration register
#bit GIE = INTCON.7
#bit PEIE = INTCON.6
#bit TMR0IE = INTCON.5
#bit INT0IE = INTCON.4
#bit RBIE = INTCON.3
#bit TMR0IF = INTCON.2
#bit INT0IF = INTCON.1
#bit RBIF = INTCON.0
 
enum stavy {start,rozjezd,jizda,cihla,cil};
stavy stav; // Kde jsme na trati
int8 cas; // Cas hrany bila/cerna v radce
 
void SetServo(int8 angle)
{
int8 n, offset;
for(n=0; n<20; n++)
{
set_adc_channel(MODRA); // Kroutitko na vystredeni predniho kolecka
Delay_ms(1);
offset=read_adc();
 
output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo
delay_us(1000);
delay_us(offset);
delay_us(offset);
delay_us(offset);
delay_us(angle);
delay_us(angle);
output_low(SERVO);
delay_ms(18);
}
}
 
#int_EXT
EXT_isr() // Preruseni od prekazky
{
set_pwm1_duty(0); // reverz (zabrzdi)
set_pwm2_duty(0);
output_high(MOT_L);
output_high(MOT_R);
delay_ms(400);
output_low(MOT_L); // zastav
output_low(MOT_R);
if (stav==cihla) while(true); // Zastav na furt
if(stav==jizda)
{
int n;
 
SetServo(CASAVR-CASMIN);
set_pwm1_duty(40); // pomalu couvej
set_pwm2_duty(40);
output_high(MOT_L);
output_high(MOT_R);
n=0;
while(true)
{
while(input(ODO));
while(!input(ODO));
n++;
if(n==6) break;
}
set_pwm1_duty(255); // reverz (zabrzdi)
set_pwm2_duty(255);
output_low(MOT_L);
output_low(MOT_R);
delay_ms(100);
set_pwm1_duty(0); // Zastav
set_pwm2_duty(0);
output_low(MOT_L);
output_low(MOT_R);
delay_ms(1000);
 
SetServo((CASAVR-CASMIN)-20); // doleva
set_pwm1_duty(150); // vpred
set_pwm2_duty(200);
output_low(MOT_L);
output_low(MOT_R);
stav=cihla;
n=0;
while(true)
{
while(input(ODO));
while(!input(ODO));
n++;
if(n==10) break;
}
set_pwm1_duty(0); // reverz (zabrzdi)
set_pwm2_duty(0);
output_high(MOT_L);
output_high(MOT_R);
 
SetServo((CASAVR-CASMIN)); // rovne
set_pwm1_duty(140); // vpred
set_pwm2_duty(140);
output_low(MOT_L);
output_low(MOT_R);
stav=cihla;
n=0;
while(true)
{
while(input(ODO));
while(!input(ODO));
n++;
if(n==10) break;
}
set_pwm1_duty(0); // reverz (zabrzdi)
set_pwm2_duty(0);
output_high(MOT_L);
output_high(MOT_R);
delay_ms(200);
set_pwm1_duty(150); // Zastav
set_pwm2_duty(150);
output_high(MOT_L);
output_high(MOT_R);
delay_ms(1000);
 
SetServo(CASMIN); // max. doleva
set_pwm1_duty(0); // vzad
set_pwm2_duty(20);
output_low(MOT_L);
output_high(MOT_R);
stav=cihla;
n=0;
while(true)
{
while(input(ODO));
while(!input(ODO));
n++;
if(n==10) break;
}
set_pwm1_duty(0); // reverz (zabrzdi)
set_pwm2_duty(255);
output_low(MOT_L);
output_low(MOT_R);
delay_ms(100);
set_pwm1_duty(0); // Zastav
set_pwm2_duty(0);
output_low(MOT_L);
output_low(MOT_R);
delay_ms(500);
 
 
SetServo((CASAVR-CASMIN)+5); // mirne doprava
set_pwm1_duty(180); // vpred
set_pwm2_duty(180);
output_low(MOT_L);
output_low(MOT_R);
stav=cihla;
n=0;
while(true)
{
while(input(ODO));
while(!input(ODO));
n++;
if(n==10) break;
}
set_pwm1_duty(0); // Zastav
set_pwm2_duty(0);
output_low(MOT_L);
output_low(MOT_R);
delay_ms(500);
 
 
cas=CASMIN; // Cara je vlevo
stav=cihla;
}
}
 
// Zobrazeni jednoho byte na modulu LEDbar
void disp(int8 x)
{
39,7 → 212,7
}
 
// Blikani LEDbarem ve stilu Night Rider
void NightRider(int x)
void NightRider(int8 x)
{
int n,i,j;
 
50,30 → 223,28
{
disp(i);
rotate_left(&i, 1);
delay_ms(50);
delay_ms(40);
}
for(n=0;n<7;n++)
{
disp(i);
rotate_right(&i, 1);
delay_ms(50);
delay_ms(40);
}
}
disp(i);
delay_ms(30);
delay_ms(40);
i=0;
disp(i);
}
 
 
void main()
{
int8 cas; // Cas hrany bila/cerna v radce
int8 offset; // Promena pro ulozeni ovsetu
int8 rr; // Promenna na ulozeni Rozumne rychlost
int8 r1; // Rychlost motoru 1
int8 r2; // Rychlost motoru 2
enum stavy {start,rozjezd,jizda,cihla,cil};
stavy stav; // Kde jsme na trati
int16 trasa; // Pocitadlo ujete vzdalenosti
 
setup_adc_ports(ALL_ANALOG); // Zapnuti A/D prevodniku pro cteni kroutitek
151,7 → 322,7
 
{ // Kalibrace kamery
int8 t1,t2;
 
for(offset=0;offset<=255;offset+=0x04) // Cita porad dokola
{
i2c_start(); // Brightness
160,23 → 331,23
i2c_write(offset); // 80h default
i2c_stop();
disp(offset);
delay_ms(100);
delay_ms(50);
 
t1=0;
t2=0;
while(!input(HREF)); // Cekej nez se zacnou posilat pixely z radky
delay_ms(5);
delay_ms(5);
while(!input(HREF)); // Cekej nez se zacnou posilat pixely z radky
set_timer0(0); // Vynuluj pocitadlo casu
if(!input(PIX)) continue;
while(input(PIX));
t1=get_timer0(); // Precti cas z citace casu hrany
set_timer0(0); // Vynuluj pocitadlo casu
set_timer0(0); // Vynuluj pocitadlo casu
while(!input(PIX));
t2=get_timer0();
t2=get_timer0();
 
if((t1>60) && (t1<140) && (t2>5) && (t2<=10)) break;
 
delay_ms(2);
}
}
183,7 → 354,7
 
delay_ms(1000);
 
cas=128; // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
cas=CASAVR-CASMIN; // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
stav=start;
trasa=0;
 
199,7 → 370,7
while(input(HREF)) // Po dobu vysilani radky cekej na hranu W/B
{
// !!!!Dodelat rozpoznani cerne cary napric pro zastaveni
if(!input(PIX)) // Pokud se 3x za sebou precetla CERNA
if(!input(PIX)) // Pokud se Xx za sebou precetla CERNA
if(!input(PIX))
// if(!input(PIX))
{
221,6 → 392,7
delay_us(1000);
delay_us(offset);
delay_us(offset);
delay_us(offset);
delay_us(cas);
delay_us(cas);
output_low(SERVO);
240,7 → 412,7
// r1<<=1; // Rychlost je dvojnasobna
// r2<<=1; // Rozsah 2 az 184
 
if (stav==jizda) // Jizda
if ((stav==jizda)||(stav==cihla)) // Jizda
{
set_pwm1_duty(r1);
set_pwm2_duty(r2);
254,43 → 426,34
 
set_adc_channel(DALKOMER); // Prepni A/D prevodnik na detektor cihly
Delay_ms(1);
for(n=0;n<200;n++) // Detekce prekazky
 
if((stav==jizda)&&(trasa>50)) // musi to alespon 1s jet
{
if(read_adc()<128)
{
if(stav==start)
{
disp(0x55);
while(read_adc()<128); // Cekej, dokud starter neda ruku pryc
set_pwm1_duty(255);
set_pwm2_duty(255);
disp(0xAA);
delay_ms(500);
stav=jizda;
break;
};
if((stav==jizda)&&(trasa>50)) // musi to alespon 1s jet
{
set_pwm1_duty(0);
set_pwm2_duty(0);
output_high(MOT_L); // zabrzdi
output_high(MOT_R);
delay_ms(300);
output_low(MOT_L); // zastav
output_low(MOT_R);
stav=cil;
while(true);
};
}
ext_int_edge(H_TO_L); // Nastav podminky preruseni od cihly
INT0IF=0; // Zruseni predesle udalosti od startera
enable_interrupts(INT_EXT);
enable_interrupts(GLOBAL);
};
 
if(stav==start)
if(read_adc()<128)
{
disp(0x55);
while(read_adc()<128); // Cekej, dokud starter neda ruku pryc
set_pwm1_duty(255);
set_pwm2_duty(255);
disp(0xAA);
delay_ms(200);
stav=jizda;
}
 
pom=0x80;
pom=0x80;
for(n=CASMAX/8; n<cas; n+=CASMAX/8)
{
pom>>=1;
}
disp(pom);
 
output_high(PIN_C0);
delay_ms(1);
output_low(PIN_C0);