Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 179 → Rev 180

/roboti/istrobot/camerus/SW/876/camerus.c
2,22 → 2,22
 
#USE FAST_IO (C) // Brana C je ve FAST_IO modu, aby slo rychle cteni z kamery
 
// kroutitka
#define CERVENA 3 // AN3/RA3
#define CERNA 2 // AN2/RA2
#define ZELENA 1 // AN1/RA0
#define MODRA 0 // AN0/RA1
// A/D vstupy
#define DALKOMER 4 // AN4/RA5 - dalkomer na cihlu
#define CERNA 3 // AN3/RA3
#define CERVENA 2 // AN2/RA2 - cervene kroutitko
#define ZELENA 1 // AN1/RA0 - zelene kroutitko
#define MODRA 0 // AN0/RA1 - modre kroutitko
 
// I/O
#define LED PIN_C0 // LED signalizujici start programu
#define HREF PIN_C5 // Signal HREF z kamery (v H po celou dobu radku)
#define PIX PIN_C6 // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))
#define SERVO PIN_B4 // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))
#define MOT_L PIN_B5 // Smer otaceni leveho motoru; druhy pol je RC2
#define MOT_R PIN_B6 // Smer otaceni praveho motoru; druhy pol je RC1
 
#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 CASMIN 6 // Rozsah radku snimace
#define CASMAX 192
72,9 → 72,10
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
 
int16 ble; // Prodleva do rozjezdu
 
setup_adc_ports(ALL_ANALOG); // Zapnuti A/D prevodniku pro cteni kroutitek
setup_adc(ADC_CLOCK_INTERNAL);
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro mereni casu hrany W/B v radce
143,13 → 144,15
NightRider(1); // Musi se dat cas kamere na AGC a AEC
 
cas=128; // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
ble=0;
 
stav=start;
trasa=0;
// ... Hlavni smycka ...
while(true)
{
int8 pom;
 
int8 n;
pom=0;
while(!input(HREF)); // Cekej nez se zacnou posilat pixely z radky
set_timer0(0); // Vynuluj pocitadlo casu
182,35 → 185,67
delay_us(cas);
output_low(SERVO);
 
ble++; // Casovac pro odlozeni rozjezdu po zapnuti napajeni
set_adc_channel(ZELENA); // Kroutitko pro vykon motoru
Delay_ms(1);
rr=read_adc()>>2; //!!! // 0-31
 
disp(cas & 0b11110000);
 
// Elektronicky diferencial
if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
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);
if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
 
// r1<<=1; // Rychlost je dvojnasobna
// r2<<=1; // Rozsah 2 az 184
 
if ((ble>100)&&(ble<60000)) // Jizda
if (stav==jizda) // Jizda
{
set_pwm1_duty(r1);
set_pwm2_duty(r2);
trasa++;
}
else
{
//!!!!!!!!!!!!!!!!!! Tenhle kod pred zavodem zrusit !!!!!!!!!!!!!!!!!!!!!!!!!!!!
set_pwm1_duty(0); // Zastaveni
set_pwm2_duty(0);
}
 
set_adc_channel(DALKOMER); // Prepni A/D prevodnik na detektor cihly
Delay_ms(1);
for(n=0;n<5;n++) // Detekce prekazky
{
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);
};
}
}
 
};
}