| 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); |
| }; |
| } |
| } |
| |
| }; |
| } |