| 0,0 → 1,159 |
|---|
| #include ".\camerus.h" |
| |
| #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 |
| |
| // 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_B7 // 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 |
| |
| 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 |
| |
| 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 |
| setup_timer_1(T1_DISABLED); |
| setup_timer_2(T2_DIV_BY_16,255,1); // Casovac PWM motoru |
| setup_ccp1(CCP_PWM); // RC1 // PWM pro motory |
| setup_ccp2(CCP_PWM); // RC2 |
| setup_comparator(NC_NC_NC_NC); |
| setup_vref(FALSE); |
| |
| set_tris_c(0b11111000); // Nastaveni vstup/vystup pro branu C, protoze se nedela automaticky |
| |
| set_pwm1_duty(0); // Zastav motory |
| set_pwm2_duty(0); |
| output_low(MOT_L); |
| output_low(MOT_R); |
| |
| output_high(LED); // Budeme blikat LED, aby se poznalo, ze byl RESET |
| |
| delay_ms(200); // Musi se pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy |
| |
| //... Nastaveni kamery ... |
| i2c_start(); // Soft RESET kamery |
| i2c_write(0xC0); // Pro single slave musi mit vsechny zapisy adresu C0h |
| i2c_write(0x12); // Adresa registru COMH |
| i2c_write(0x80 | 0x24); // Zapis ridiciho slova |
| i2c_stop(); |
| |
| i2c_start(); // BW |
| i2c_write(0xC0); |
| i2c_write(0x28); |
| i2c_write(0b01000001); |
| i2c_stop(); |
| |
| |
| i2c_start(); // Contrast |
| i2c_write(0xC0); |
| i2c_write(0x05); |
| i2c_write(0xA0); |
| i2c_stop(); |
| /* |
| i2c_start(); // Brightness |
| i2c_write(0xC0); |
| i2c_write(0x06); |
| i2c_write(0x80); |
| i2c_stop(); |
| |
| i2c_start(); // Band Filter |
| i2c_write(0xC0); |
| i2c_write(0x2D); |
| i2c_write(0x04 | 0x03); |
| i2c_stop(); |
| */ |
| i2c_start(); // VSTRT |
| i2c_write(0xC0); |
| i2c_write(0x19); |
| i2c_write(45); |
| i2c_stop(); |
| |
| i2c_start(); // VEND |
| i2c_write(0xC0); |
| i2c_write(0x1A); |
| i2c_write(45); |
| i2c_stop(); |
| |
| output_low(LED); // Blikani LED |
| delay_ms(200); |
| |
| output_high(LED); // Blikani LED |
| delay_ms(200); |
| output_low(LED); |
| |
| cas=128; // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska |
| ble=0; |
| |
| // ... Hlavni smycka ... |
| while(true) |
| { |
| while(!input(HREF)); // Cekej nez se zacnou posilat pixely z radky |
| set_timer0(0); // Vynuluj pocitadlo casu |
| while(input(HREF)) // Po dobu vysilani radky cekej na hranu W/B |
| { |
| if(!input(PIX)) // Pokud se dvakrat za sebou precetla CERNA |
| if(!input(PIX)) |
| { |
| cas=get_timer0(); // Precti cas z citace casu hrany |
| cas=255-cas; // Vlevo je vpravo |
| break; |
| }; |
| }; |
| |
| 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(cas); |
| delay_us(cas); |
| output_low(SERVO); |
| |
| ble++; // Casovac pro odlozeni rozjezdu po zapnuti napajeni |
| set_adc_channel(ZELENA); // Kroutitka pro vykon motoru (!!! to bude chtit jeste predelat !!!) |
| Delay_ms(1); |
| offset=read_adc(); |
| set_adc_channel(CERNA); |
| Delay_ms(1); |
| rr=read_adc(); |
| |
| r1=cas>>1; // Elektronicky diferencial |
| r2=255-offset-(cas>>1); |
| |
| if (ble==99) // Rozjezd na Rozumnou rychlost |
| { |
| set_pwm1_duty(rr); |
| set_pwm2_duty(rr); |
| }; |
| if ((ble>100)&&(ble<60000)) // Jizda |
| { |
| if (r1<=rr) set_pwm1_duty(r1); |
| if (r2<=rr) set_pwm2_duty(r2); |
| } |
| else |
| { |
| set_pwm1_duty(0); // Zastaveni |
| set_pwm2_duty(0); |
| } |
| }; |
| } |