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,12 → 144,14 |
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 |
182,7 → 185,6 |
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 |
201,16 → 203,49 |
// 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); |
}; |
} |
} |
|
}; |
} |