| 177 |
kaklik |
1 |
#include ".\camerus.h"
|
|
|
2 |
|
|
|
3 |
#USE FAST_IO (C) // Brana C je ve FAST_IO modu, aby slo rychle cteni z kamery
|
|
|
4 |
|
|
|
5 |
// kroutitka
|
|
|
6 |
#define CERVENA 3 // AN3/RA3
|
|
|
7 |
#define CERNA 2 // AN2/RA2
|
|
|
8 |
#define ZELENA 1 // AN1/RA0
|
|
|
9 |
#define MODRA 0 // AN0/RA1
|
|
|
10 |
|
|
|
11 |
// I/O
|
|
|
12 |
#define LED PIN_C0 // LED signalizujici start programu
|
|
|
13 |
#define HREF PIN_C5 // Signal HREF z kamery (v H po celou dobu radku)
|
|
|
14 |
#define PIX PIN_C6 // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))
|
|
|
15 |
#define SERVO PIN_B7 // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))
|
|
|
16 |
#define MOT_L PIN_B5 // Smer otaceni leveho motoru; druhy pol je RC2
|
|
|
17 |
#define MOT_R PIN_B6 // Smer otaceni praveho motoru; druhy pol je RC1
|
|
|
18 |
|
|
|
19 |
void main()
|
|
|
20 |
{
|
|
|
21 |
int8 cas; // Cas hrany bila/cerna v radce
|
|
|
22 |
int8 offset; // Promena pro ulozeni ovsetu
|
|
|
23 |
int8 rr; // Promenna na ulozeni Rozumne rychlost
|
|
|
24 |
int8 r1; // Rychlost motoru 1
|
|
|
25 |
int8 r2; // Rychlost motoru 2
|
|
|
26 |
|
|
|
27 |
int16 ble; // Prodleva do rozjezdu
|
|
|
28 |
|
|
|
29 |
setup_adc_ports(ALL_ANALOG); // Zapnuti A/D prevodniku pro cteni kroutitek
|
|
|
30 |
setup_adc(ADC_CLOCK_INTERNAL);
|
|
|
31 |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro mereni casu hrany W/B v radce
|
|
|
32 |
setup_timer_1(T1_DISABLED);
|
|
|
33 |
setup_timer_2(T2_DIV_BY_16,255,1); // Casovac PWM motoru
|
|
|
34 |
setup_ccp1(CCP_PWM); // RC1 // PWM pro motory
|
|
|
35 |
setup_ccp2(CCP_PWM); // RC2
|
|
|
36 |
setup_comparator(NC_NC_NC_NC);
|
|
|
37 |
setup_vref(FALSE);
|
|
|
38 |
|
|
|
39 |
set_tris_c(0b11111000); // Nastaveni vstup/vystup pro branu C, protoze se nedela automaticky
|
|
|
40 |
|
|
|
41 |
set_pwm1_duty(0); // Zastav motory
|
|
|
42 |
set_pwm2_duty(0);
|
|
|
43 |
output_low(MOT_L);
|
|
|
44 |
output_low(MOT_R);
|
|
|
45 |
|
|
|
46 |
output_high(LED); // Budeme blikat LED, aby se poznalo, ze byl RESET
|
|
|
47 |
|
|
|
48 |
delay_ms(200); // Musi se pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
|
|
|
49 |
|
|
|
50 |
//... Nastaveni kamery ...
|
|
|
51 |
i2c_start(); // Soft RESET kamery
|
|
|
52 |
i2c_write(0xC0); // Pro single slave musi mit vsechny zapisy adresu C0h
|
|
|
53 |
i2c_write(0x12); // Adresa registru COMH
|
|
|
54 |
i2c_write(0x80 | 0x24); // Zapis ridiciho slova
|
|
|
55 |
i2c_stop();
|
|
|
56 |
|
|
|
57 |
i2c_start(); // BW
|
|
|
58 |
i2c_write(0xC0);
|
|
|
59 |
i2c_write(0x28);
|
|
|
60 |
i2c_write(0b01000001);
|
|
|
61 |
i2c_stop();
|
|
|
62 |
|
|
|
63 |
|
|
|
64 |
i2c_start(); // Contrast
|
|
|
65 |
i2c_write(0xC0);
|
|
|
66 |
i2c_write(0x05);
|
|
|
67 |
i2c_write(0xA0);
|
|
|
68 |
i2c_stop();
|
|
|
69 |
/*
|
|
|
70 |
i2c_start(); // Brightness
|
|
|
71 |
i2c_write(0xC0);
|
|
|
72 |
i2c_write(0x06);
|
|
|
73 |
i2c_write(0x80);
|
|
|
74 |
i2c_stop();
|
|
|
75 |
|
|
|
76 |
i2c_start(); // Band Filter
|
|
|
77 |
i2c_write(0xC0);
|
|
|
78 |
i2c_write(0x2D);
|
|
|
79 |
i2c_write(0x04 | 0x03);
|
|
|
80 |
i2c_stop();
|
|
|
81 |
*/
|
|
|
82 |
i2c_start(); // VSTRT
|
|
|
83 |
i2c_write(0xC0);
|
|
|
84 |
i2c_write(0x19);
|
|
|
85 |
i2c_write(45);
|
|
|
86 |
i2c_stop();
|
|
|
87 |
|
|
|
88 |
i2c_start(); // VEND
|
|
|
89 |
i2c_write(0xC0);
|
|
|
90 |
i2c_write(0x1A);
|
|
|
91 |
i2c_write(45);
|
|
|
92 |
i2c_stop();
|
|
|
93 |
|
|
|
94 |
output_low(LED); // Blikani LED
|
|
|
95 |
delay_ms(200);
|
|
|
96 |
|
|
|
97 |
output_high(LED); // Blikani LED
|
|
|
98 |
delay_ms(200);
|
|
|
99 |
output_low(LED);
|
|
|
100 |
|
|
|
101 |
cas=128; // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
|
|
|
102 |
ble=0;
|
|
|
103 |
|
|
|
104 |
// ... Hlavni smycka ...
|
|
|
105 |
while(true)
|
|
|
106 |
{
|
|
|
107 |
while(!input(HREF)); // Cekej nez se zacnou posilat pixely z radky
|
|
|
108 |
set_timer0(0); // Vynuluj pocitadlo casu
|
|
|
109 |
while(input(HREF)) // Po dobu vysilani radky cekej na hranu W/B
|
|
|
110 |
{
|
|
|
111 |
if(!input(PIX)) // Pokud se dvakrat za sebou precetla CERNA
|
|
|
112 |
if(!input(PIX))
|
|
|
113 |
{
|
|
|
114 |
cas=get_timer0(); // Precti cas z citace casu hrany
|
|
|
115 |
cas=255-cas; // Vlevo je vpravo
|
|
|
116 |
break;
|
|
|
117 |
};
|
|
|
118 |
};
|
|
|
119 |
|
|
|
120 |
set_adc_channel(MODRA); // Kroutitko na vystredeni predniho kolecka
|
|
|
121 |
Delay_ms(1);
|
|
|
122 |
offset=read_adc();
|
|
|
123 |
|
|
|
124 |
output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo
|
|
|
125 |
delay_us(1000);
|
|
|
126 |
delay_us(offset);
|
|
|
127 |
delay_us(offset);
|
|
|
128 |
delay_us(cas);
|
|
|
129 |
delay_us(cas);
|
|
|
130 |
output_low(SERVO);
|
|
|
131 |
|
|
|
132 |
ble++; // Casovac pro odlozeni rozjezdu po zapnuti napajeni
|
|
|
133 |
set_adc_channel(ZELENA); // Kroutitka pro vykon motoru (!!! to bude chtit jeste predelat !!!)
|
|
|
134 |
Delay_ms(1);
|
|
|
135 |
offset=read_adc();
|
|
|
136 |
set_adc_channel(CERNA);
|
|
|
137 |
Delay_ms(1);
|
|
|
138 |
rr=read_adc();
|
|
|
139 |
|
|
|
140 |
r1=cas>>1; // Elektronicky diferencial
|
|
|
141 |
r2=255-offset-(cas>>1);
|
|
|
142 |
|
|
|
143 |
if (ble==99) // Rozjezd na Rozumnou rychlost
|
|
|
144 |
{
|
|
|
145 |
set_pwm1_duty(rr);
|
|
|
146 |
set_pwm2_duty(rr);
|
|
|
147 |
};
|
|
|
148 |
if ((ble>100)&&(ble<60000)) // Jizda
|
|
|
149 |
{
|
|
|
150 |
if (r1<=rr) set_pwm1_duty(r1);
|
|
|
151 |
if (r2<=rr) set_pwm2_duty(r2);
|
|
|
152 |
}
|
|
|
153 |
else
|
|
|
154 |
{
|
|
|
155 |
set_pwm1_duty(0); // Zastaveni
|
|
|
156 |
set_pwm2_duty(0);
|
|
|
157 |
}
|
|
|
158 |
};
|
|
|
159 |
}
|