| 591 |
kakl |
1 |
//********* Robot Camerus pro IstRobot 2007 ************ |
|
|
2 |
//"$Id: camerus.c 253 2007-04-24 09:26:46Z kakl $" |
|
|
3 |
//***************************************************** |
|
|
4 |
|
|
|
5 |
#include ".\camerus.h" |
|
|
6 |
|
|
|
7 |
#USE FAST_IO (C) // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery |
|
|
8 |
|
|
|
9 |
// Rychlostni konstanty |
|
|
10 |
#define RR_CIHLA 50 // Rozumna rychlost pro objizdeni cihly |
|
|
11 |
#define RR_PRERUSENI 50 // Rozumna rychlost pro priblizeni se k preruseni |
|
|
12 |
#define BRZDNA_DRAHA 0x15 // Jak daleko pred problemem se zacne brzdit |
|
|
13 |
#define TUHOS 100 // Jak dlouho se bude couvat po narazu na naraznik |
|
|
14 |
#define ODODO_PROBLEM1 0xFFF |
|
|
15 |
#define ODODO_PROBLEM2 0xFFF |
|
|
16 |
#define ODODO_PROBLEM3 0xFFF |
|
|
17 |
|
|
|
18 |
// Adresy IIC periferii |
|
|
19 |
#define COMPAS_ADR 0xC0 |
|
|
20 |
#define CAMERA_ADR 0xDC |
|
|
21 |
#define SONAR_ADR 0xE0 |
|
|
22 |
|
|
|
23 |
// A/D vstupy |
|
|
24 |
#define RMAX 4 // AN4/RA5 - leve cidlo na vyjeti z cary |
|
|
25 |
#define LMAX 3 // AN3/RA3 - prave cidlo na vyjeti z cary |
|
|
26 |
#define CERVENA 2 // AN2/RA2 - cervene kroutitko |
|
|
27 |
#define ZELENA 1 // AN1/RA0 - zelene kroutitko |
|
|
28 |
#define MODRA 0 // AN0/RA1 - modre kroutitko |
|
|
29 |
|
|
|
30 |
// I/O |
|
|
31 |
#define HREF PIN_C5 // Signal HREF z kamery (v H po celou dobu radku) |
|
|
32 |
#define PIX PIN_C6 // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator)) |
|
|
33 |
#define SERVO PIN_B4 // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem)) |
|
|
34 |
#define MOT_L PIN_B5 // Smer otaceni leveho motoru; druhy pol je RC2 |
|
|
35 |
#define MOT_R PIN_B6 // Smer otaceni praveho motoru; druhy pol je RC1 |
|
|
36 |
#define MOT_1 PIN_C1 // PWM vystpy motoru |
|
|
37 |
#define MOT_2 PIN_C2 // |
|
|
38 |
#define DATA PIN_B2 // K modulu LEDbar data |
|
|
39 |
#define CP PIN_B1 // K modulu LEDbar hodiny |
|
|
40 |
//#define ODO PIN_C0 // Ze snimace z odometrie z praveho kola na TIMER1 |
|
|
41 |
// Jeden impuls je 31,25mm |
|
|
42 |
#define IRRX !input(PIN_B0) // Vstup INT, generuje preruseni pri prekazce |
|
|
43 |
#define IRTX PIN_B3 // Modulovani vysilaci IR LED na detekci prekazky |
|
|
44 |
#define PROXIMITY PIN_C7 // Cidlo kratkeho dosahu na cihlu |
|
|
45 |
#define BUMPER !input(PIN_A4) // Naraznik |
|
|
46 |
|
|
|
47 |
#define CASMIN 6 // Rozsah radku snimace |
|
|
48 |
#define CASMAX 192 |
|
|
49 |
#define CASAVR ((CASMAX+CASMIN) / 2) |
|
|
50 |
|
|
|
51 |
#define EEMAX 255 // Konec EEPROM |
|
|
52 |
#define MAXLOG 0x10 // Maximalni pocet zaznamu v logu |
|
|
53 |
#if MAXLOG>(EEMAX/3) |
|
|
54 |
#error Prekrocena velikost EEPROM |
|
|
55 |
#endif |
|
|
56 |
|
|
|
57 |
#define OFFSETO 0x9F //0x9F // Vystredeni serva pro objeti prekazky |
|
|
58 |
|
|
|
59 |
#define THR 90 // Threshold pro UV cidla na caru |
|
|
60 |
|
|
|
61 |
#byte INTCON = 0x0B // Interrupt configuration register |
|
|
62 |
#bit GIE = INTCON.7 |
|
|
63 |
#bit PEIE = INTCON.6 |
|
|
64 |
#bit TMR0IE = INTCON.5 |
|
|
65 |
#bit INT0IE = INTCON.4 |
|
|
66 |
#bit RBIE = INTCON.3 |
|
|
67 |
#bit TMR0IF = INTCON.2 |
|
|
68 |
#bit INT0IF = INTCON.1 |
|
|
69 |
#bit RBIF = INTCON.0 |
|
|
70 |
|
|
|
71 |
enum stavy {start,rozjezd,jizda,cihla,pocihle,cil}; |
|
|
72 |
stavy stav; // Kde jsme na trati |
|
|
73 |
int8 cas; // Cas hrany bila/cerna v radce |
|
|
74 |
int8 stred; // Vystredeni kolecka |
|
|
75 |
int16 odocounter; // Zaznamenani aktualniho stavu pocitadla odometrie |
|
|
76 |
int16 last_log_odo; // Posledni stav odometrie poznamenany do logu |
|
|
77 |
int16 last_log; // Cislo posledniho zaznamu v logu v EEPROM |
|
|
78 |
int8 bb_h[MAXLOG]; // Cerna skrinka MSB |
|
|
79 |
int8 bb_l[MAXLOG]; // Cerna skrinka LSB |
|
|
80 |
int8 bb_f[MAXLOG]; // Cerna skrinka priznak (typ zaznamu) |
|
|
81 |
int8 log; // Pocitadlo pro cernou skrinku |
|
|
82 |
int8 rr; // Promenna na ulozeni Rozumne rychlost |
|
|
83 |
int8 rrold; |
|
|
84 |
int16 odo_preruseni, odo_cihla, odo_tunel; // Problemy na trati |
|
|
85 |
int16 odo_problem1, odo_problem2, odo_problem3; // Problemy na trati |
|
|
86 |
|
|
|
87 |
// Zobrazeni jednoho byte na modulu LEDbar |
|
|
88 |
inline void disp(int8 x) |
|
|
89 |
{ |
|
|
90 |
int n; |
|
|
91 |
|
|
|
92 |
for(n=0;n<=7;n++) |
|
|
93 |
{ |
|
|
94 |
if (bit_test(x,0)) output_low(DATA); else output_high(DATA); |
|
|
95 |
output_high(CP); |
|
|
96 |
x>>=1; |
|
|
97 |
output_low(CP); |
|
|
98 |
} |
|
|
99 |
} |
|
|
100 |
|
|
|
101 |
// Blikani LEDbarem ve stilu Night Rider |
|
|
102 |
void NightRider(int8 x) |
|
|
103 |
{ |
|
|
104 |
int n,i,j; |
|
|
105 |
|
|
|
106 |
for(j=0;j<x;j++) |
|
|
107 |
{ |
|
|
108 |
i=0x01; |
|
|
109 |
for(n=0;n<7;n++) |
|
|
110 |
{ |
|
|
111 |
disp(i); |
|
|
112 |
rotate_left(&i, 1); |
|
|
113 |
delay_ms(40); |
|
|
114 |
} |
|
|
115 |
for(n=0;n<7;n++) |
|
|
116 |
{ |
|
|
117 |
disp(i); |
|
|
118 |
rotate_right(&i, 1); |
|
|
119 |
delay_ms(40); |
|
|
120 |
} |
|
|
121 |
} |
|
|
122 |
disp(i); |
|
|
123 |
delay_ms(40); |
|
|
124 |
i=0; |
|
|
125 |
disp(i); |
|
|
126 |
} |
|
|
127 |
|
|
|
128 |
// Zaznam LOGu do EEPROM |
|
|
129 |
void SaveLog() |
|
|
130 |
{ |
|
|
131 |
int8 n,i,xlog; |
|
|
132 |
|
|
|
133 |
i=0; |
|
|
134 |
for(n=0;n<=(log*3);n+=3) // Ulozeni Black Boxu do EEPROM |
|
|
135 |
{ |
|
|
136 |
write_eeprom(n,bb_f[i]); |
|
|
137 |
write_eeprom(n+1,bb_h[i]); |
|
|
138 |
write_eeprom(n+2,bb_l[i]); |
|
|
139 |
i++; |
|
|
140 |
}; |
|
|
141 |
if(log>0) {xlog=log-1;} else {xlog=0;}; |
|
|
142 |
write_eeprom(EEMAX,xlog); // Zapis poctu zaznamu na konec EEPROM |
|
|
143 |
} |
|
|
144 |
|
|
|
145 |
// Zaznam do Logu do RAM |
|
|
146 |
void LogLog(int8 reason, int16 log_delay) |
|
|
147 |
{ |
|
|
148 |
int16 timer_pom; |
|
|
149 |
|
|
|
150 |
timer_pom=get_timer1(); // Timer se musi vycist atomicky |
|
|
151 |
bb_l[log]=make8(timer_pom,0); // Zaznam |
|
|
152 |
bb_h[log]=make8(timer_pom,1); |
|
|
153 |
bb_f[log]=reason; // Typ zaznamu |
|
|
154 |
if(log<(MAXLOG-1)) log++; // Ukazatel na dalsi zaznam |
|
|
155 |
last_log_odo=timer_pom+log_delay; // Dalsi mereni nejdrive po ujeti def. vzdalenosti |
|
|
156 |
rr=rrold; // Problem skoncil, znovu jed Rozumnou Rychlosti |
|
|
157 |
} |
|
|
158 |
|
|
|
159 |
void ReadBlackBox() |
|
|
160 |
{ |
|
|
161 |
last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano? |
|
|
162 |
{ |
|
|
163 |
int8 n,i; |
|
|
164 |
|
|
|
165 |
i=0; |
|
|
166 |
for(n=0;n<=last_log;n++) |
|
|
167 |
{ |
|
|
168 |
if(read_eeprom(i)==0) odo_tunel=MAKE16(read_eeprom(i+2),read_eeprom(i+1)); |
|
|
169 |
if(read_eeprom(i)==0xFF) odo_cihla=MAKE16(read_eeprom(i+2),read_eeprom(i+1)); |
|
|
170 |
if((read_eeprom(i)>0) && (read_eeprom(i)<0xFF)) odo_preruseni=MAKE16(read_eeprom(i+2),read_eeprom(i+1)); |
|
|
171 |
} |
|
|
172 |
} |
|
|
173 |
} |
|
|
174 |
|
|
|
175 |
|
|
|
176 |
// Brzdeni motorama stridou 1:1 |
|
|
177 |
void brzda() |
|
|
178 |
{ |
|
|
179 |
int8 n,i; |
|
|
180 |
|
|
|
181 |
set_pwm1_duty(0); // vypni PWM |
|
|
182 |
set_pwm2_duty(0); |
|
|
183 |
setup_ccp1(CCP_OFF); |
|
|
184 |
setup_ccp2(CCP_OFF); |
|
|
185 |
for (n=0;n<200;n++) |
|
|
186 |
{ |
|
|
187 |
output_low(MOT_L); |
|
|
188 |
output_low(MOT_R); |
|
|
189 |
output_high(MOT_1); |
|
|
190 |
output_high(MOT_2); |
|
|
191 |
delay_us(200); |
|
|
192 |
output_high(MOT_L); |
|
|
193 |
output_high(MOT_R); |
|
|
194 |
output_low(MOT_1); |
|
|
195 |
output_low(MOT_2); |
|
|
196 |
delay_us(200); |
|
|
197 |
} |
|
|
198 |
output_low(MOT_L); // smer vpred |
|
|
199 |
output_low(MOT_R); |
|
|
200 |
setup_ccp1(CCP_PWM); // RC1 // Zapni PWM pro motory |
|
|
201 |
setup_ccp2(CCP_PWM); // RC2 |
|
|
202 |
} |
|
|
203 |
|
|
|
204 |
void SetServo(int8 angle) |
|
|
205 |
{ |
|
|
206 |
int8 n; |
|
|
207 |
|
|
|
208 |
for(n=0; n<10; n++) |
|
|
209 |
{ |
|
|
210 |
output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo |
|
|
211 |
delay_us(1000); |
|
|
212 |
delay_us(stred); |
|
|
213 |
delay_us(stred); |
|
|
214 |
delay_us(stred); |
|
|
215 |
delay_us(angle); |
|
|
216 |
delay_us(angle); |
|
|
217 |
output_low(SERVO); |
|
|
218 |
delay_ms(18); |
|
|
219 |
} |
|
|
220 |
} |
|
|
221 |
|
|
|
222 |
inline void SetServoQ(int8 angle) |
|
|
223 |
{ |
|
|
224 |
output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo |
|
|
225 |
delay_us(1000); |
|
|
226 |
delay_us(stred); |
|
|
227 |
delay_us(stred); |
|
|
228 |
delay_us(stred); |
|
|
229 |
delay_us(angle); |
|
|
230 |
delay_us(angle); |
|
|
231 |
output_low(SERVO); |
|
|
232 |
} |
|
|
233 |
|
|
|
234 |
// Couvni po narazu na naraznik |
|
|
235 |
inline void bum() |
|
|
236 |
{ |
|
|
237 |
set_pwm1_duty(0); // couvni, rovne dozadu |
|
|
238 |
set_pwm2_duty(0); |
|
|
239 |
output_high(MOT_L); |
|
|
240 |
output_high(MOT_R); |
|
|
241 |
disp(0xA5); |
|
|
242 |
SetServo(CASAVR-CASMIN); |
|
|
243 |
} |
|
|
244 |
|
|
|
245 |
#include ".\diag.c" |
|
|
246 |
|
|
|
247 |
//---------------------------- INT -------------------------------- |
|
|
248 |
#int_EXT |
|
|
249 |
EXT_isr() // Preruseni od prekazky |
|
|
250 |
{ |
|
|
251 |
unsigned int8 bearing, bearing_offset, delta_bearing; |
|
|
252 |
|
|
|
253 |
set_pwm1_duty(0); // zabrzdi levym kolem, prave vypni |
|
|
254 |
set_pwm2_duty(0); |
|
|
255 |
output_high(MOT_L); |
|
|
256 |
output_low(MOT_R); |
|
|
257 |
// Ujistime se, ze prijaty signal je z naseho IR vysilace |
|
|
258 |
output_high(IRTX); // Vypni LED na detekci prekazky |
|
|
259 |
delay_ms(2); |
|
|
260 |
if (IRRX) // stale nas signal? |
|
|
261 |
{ |
|
|
262 |
output_low(MOT_L); // je odraz -> vpred |
|
|
263 |
output_low(MOT_R); |
|
|
264 |
return; |
|
|
265 |
}; |
|
|
266 |
output_low(IRTX); // Zapni LED na detekci prekazky |
|
|
267 |
|
|
|
268 |
i2c_start(); // Cteni kompasu |
|
|
269 |
i2c_write(COMPAS_ADR); |
|
|
270 |
i2c_write(0x1); // 0-255 (odpovida 0-359) |
|
|
271 |
i2c_stop(); |
|
|
272 |
i2c_start(); |
|
|
273 |
i2c_write(COMPAS_ADR+1); |
|
|
274 |
bearing_offset=i2c_read(0); // Poznamenej hodnotu pred cihlou |
|
|
275 |
i2c_stop(); |
|
|
276 |
|
|
|
277 |
delay_ms(9); |
|
|
278 |
if (!IRRX) // stale nas signal? |
|
|
279 |
{ |
|
|
280 |
output_low(MOT_L); // neni odraz -> vpred |
|
|
281 |
output_low(MOT_R); |
|
|
282 |
return; |
|
|
283 |
}; |
|
|
284 |
|
|
|
285 |
rr=rrold; // Po cihle se pojede opet Rozumnou Rychlosti |
|
|
286 |
if(stav!=cihla) |
|
|
287 |
{ |
|
|
288 |
LogLog(0xFF,3); // Cihla |
|
|
289 |
}; |
|
|
290 |
|
|
|
291 |
//!!! if(stav==cihla) while(true); // Zastav na furt, konec drahy |
|
|
292 |
// if(stav==cihla) return; // Po druhe nic neobjizdej |
|
|
293 |
// Pozor na rozjezd |
|
|
294 |
|
|
|
295 |
if((stav==jizda)||(stav==cihla)) // Objed cihlu |
|
|
296 |
{ |
|
|
297 |
#include ".\objizdka_L.c" |
|
|
298 |
}; |
|
|
299 |
last_log_odo=get_timer1()+16; // Pul metru po cihle nezaznamenavej do LOGu |
|
|
300 |
} |
|
|
301 |
|
|
|
302 |
|
|
|
303 |
//---------------------------------- MAIN -------------------------------------- |
|
|
304 |
void main() |
|
|
305 |
{ |
|
|
306 |
int8 offset; // Promena pro ulozeni offsetu |
|
|
307 |
int8 r1; // Rychlost motoru 1 |
|
|
308 |
int8 r2; // Rychlost motoru 2 |
|
|
309 |
int16 ble; |
|
|
310 |
|
|
|
311 |
setup_adc_ports(ALL_ANALOG); // Zapnuti A/D prevodniku pro cteni kroutitek |
|
|
312 |
setup_adc(ADC_CLOCK_INTERNAL); |
|
|
313 |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro mereni casu hrany W/B v radce |
|
|
314 |
setup_timer_1(T1_EXTERNAL); // Cita pulzy z odometrie z praveho kola |
|
|
315 |
setup_timer_2(T2_DIV_BY_16,255,1); // Casovac PWM motoru |
|
|
316 |
//!!! setup_timer_2(T2_DIV_BY_4,255,1); // Casovac PWM motoru |
|
|
317 |
setup_ccp1(CCP_PWM); // RC1 // PWM pro motory |
|
|
318 |
setup_ccp2(CCP_PWM); // RC2 |
|
|
319 |
setup_comparator(NC_NC_NC_NC); |
|
|
320 |
setup_vref(FALSE); |
|
|
321 |
|
|
|
322 |
set_tris_c(0b11111001); // Nastaveni vstup/vystup pro branu C, protoze se to nedela automaticky |
|
|
323 |
|
|
|
324 |
set_pwm1_duty(0); // Zastav motory |
|
|
325 |
set_pwm2_duty(0); |
|
|
326 |
output_low(MOT_L); // Nastav smer vpred |
|
|
327 |
output_low(MOT_R); |
|
|
328 |
|
|
|
329 |
disp(0); // Zhasni LEDbar |
|
|
330 |
|
|
|
331 |
if(BUMPER) // Kdyz nekdo na zacatku drzi naraznik, vymaz log a spust diagnostiku |
|
|
332 |
{ |
|
|
333 |
diag(); |
|
|
334 |
} |
|
|
335 |
|
|
|
336 |
output_low(IRTX); // Zapni LED na detekci prekazky |
|
|
337 |
|
|
|
338 |
NightRider(1); // Zablikej, aby se poznalo, ze byl RESET |
|
|
339 |
// Zaroven se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy |
|
|
340 |
|
|
|
341 |
//... Nastaveni sonaru ... |
|
|
342 |
i2c_start(); |
|
|
343 |
i2c_write(SONAR_ADR); |
|
|
344 |
i2c_write(0x02); // dosah |
|
|
345 |
i2c_write(0x03); // n*43mm |
|
|
346 |
i2c_stop(); |
|
|
347 |
i2c_start(); |
|
|
348 |
i2c_write(SONAR_ADR); |
|
|
349 |
i2c_write(0x01); // zesileni |
|
|
350 |
i2c_write(0x01); // male, pro eliminaci echa z minuleho mereni |
|
|
351 |
i2c_stop(); |
|
|
352 |
|
|
|
353 |
//... Nastaveni kamery ... |
|
|
354 |
i2c_start(); // Soft RESET kamery |
|
|
355 |
i2c_write(CAMERA_ADR); // Adresa kamery |
|
|
356 |
i2c_write(0x12); // Adresa registru COMH |
|
|
357 |
i2c_write(0x80 | 0x24); // Zapis ridiciho slova |
|
|
358 |
i2c_stop(); |
|
|
359 |
|
|
|
360 |
i2c_start(); // BW |
|
|
361 |
i2c_write(CAMERA_ADR); |
|
|
362 |
i2c_write(0x28); |
|
|
363 |
i2c_write(0b01000001); |
|
|
364 |
i2c_stop(); |
|
|
365 |
|
|
|
366 |
/* |
|
|
367 |
i2c_start(); // Contrast (nema podstatny vliv na obraz) |
|
|
368 |
i2c_write(CAMERA_ADR); |
|
|
369 |
i2c_write(0x05); |
|
|
370 |
i2c_write(0xA0); // 48h |
|
|
371 |
i2c_stop(); |
|
|
372 |
|
|
|
373 |
i2c_start(); // Band Filter (pokud by byl problem se zarivkama 50Hz) |
|
|
374 |
i2c_write(CAMERA_ADR); |
|
|
375 |
i2c_write(0x2D); |
|
|
376 |
i2c_write(0x04 | 0x03); |
|
|
377 |
i2c_stop(); |
|
|
378 |
*/ |
|
|
379 |
|
|
|
380 |
i2c_start(); // Fame Rate |
|
|
381 |
i2c_write(CAMERA_ADR); |
|
|
382 |
i2c_write(0x2B); |
|
|
383 |
i2c_write(0x00); // cca 17ms (puvodni hodnota 5Eh = 20ms) |
|
|
384 |
i2c_stop(); |
|
|
385 |
|
|
|
386 |
i2c_start(); // VSTRT |
|
|
387 |
i2c_write(CAMERA_ADR); |
|
|
388 |
i2c_write(0x19); |
|
|
389 |
i2c_write(118); // prostredni radka |
|
|
390 |
i2c_stop(); |
|
|
391 |
|
|
|
392 |
i2c_start(); // VEND |
|
|
393 |
i2c_write(CAMERA_ADR); |
|
|
394 |
i2c_write(0x1A); |
|
|
395 |
i2c_write(118); |
|
|
396 |
i2c_stop(); |
|
|
397 |
|
|
|
398 |
NightRider(1); // Musi se dat cas kamere na AGC a AEC |
|
|
399 |
|
|
|
400 |
{ // Mereni expozice |
|
|
401 |
int8 t1,t2; |
|
|
402 |
|
|
|
403 |
i2c_start(); // Brightness, zacni od uplne tmy |
|
|
404 |
i2c_write(CAMERA_ADR); |
|
|
405 |
i2c_write(0x06); |
|
|
406 |
i2c_write(0); // 80h default |
|
|
407 |
i2c_stop(); |
|
|
408 |
delay_ms(50); |
|
|
409 |
|
|
|
410 |
for(offset=0x04;offset<(255-0x04);offset+=0x04) // Zacni od jasu 10h |
|
|
411 |
{ |
|
|
412 |
i2c_start(); // Brightness |
|
|
413 |
i2c_write(CAMERA_ADR); |
|
|
414 |
i2c_write(0x06); |
|
|
415 |
i2c_write(offset); // 80h default |
|
|
416 |
i2c_stop(); |
|
|
417 |
disp(offset); |
|
|
418 |
delay_ms(50); |
|
|
419 |
|
|
|
420 |
t1=0; |
|
|
421 |
t2=0; |
|
|
422 |
while(!input(HREF)); // Cekej nez se zacnou posilat pixely z radky |
|
|
423 |
delay_ms(5); |
|
|
424 |
while(!input(HREF)); // Cekej nez se zacnou posilat pixely z radky |
|
|
425 |
set_timer0(0); // Vynuluj pocitadlo casu |
|
|
426 |
if(!input(PIX)) continue; |
|
|
427 |
while(input(PIX)); |
|
|
428 |
t1=get_timer0(); // Precti cas z citace casu hrany |
|
|
429 |
set_timer0(0); // Vynuluj pocitadlo casu |
|
|
430 |
while(!input(PIX)); |
|
|
431 |
t2=get_timer0(); |
|
|
432 |
|
|
|
433 |
if((t1>60) && (t1<140) && (t2>5) && (t2<=10)) break; // Vidis, co mas? |
|
|
434 |
|
|
|
435 |
delay_ms(2); // Preskoc druhou radku z kamery |
|
|
436 |
}; |
|
|
437 |
delay_ms(1000); // Nech chvili na displayi zmerenou hodnotu |
|
|
438 |
} |
|
|
439 |
|
|
|
440 |
set_adc_channel(CERVENA); // --- Kroutitko pro jas --- |
|
|
441 |
delay_ms(1); |
|
|
442 |
offset=read_adc(); |
|
|
443 |
offset &= 0b11111100; // Dva nejnizsi bity ignoruj |
|
|
444 |
// offset += 0x70; // Jas nebude nikdy nizsi |
|
|
445 |
disp(offset); |
|
|
446 |
i2c_start(); // Brightness |
|
|
447 |
i2c_write(CAMERA_adr); |
|
|
448 |
i2c_write(0x06); |
|
|
449 |
i2c_write(offset); // 80h default |
|
|
450 |
i2c_stop(); |
|
|
451 |
delay_ms(1000); // Nech hodnotu chvili na displayi |
|
|
452 |
|
|
|
453 |
set_adc_channel(ZELENA); // --- Kroutitko pro vykon motoru --- |
|
|
454 |
delay_ms(1); |
|
|
455 |
rr=read_adc()>>2; // 0-63 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni! |
|
|
456 |
rr+=27; // 27-90 |
|
|
457 |
//!!! rr=read_adc()>>1; // 0-128 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni! |
|
|
458 |
rrold=rr; |
|
|
459 |
|
|
|
460 |
cas=CASAVR-CASMIN; // Inicializace promenych, aby neslo servo za roh |
|
|
461 |
// a aby se to rozjelo jeste dneska |
|
|
462 |
stav=start; // Jsme na startu |
|
|
463 |
set_timer1(0); // Vynuluj citac odometrie |
|
|
464 |
log=0; // Zacatek logu v cerne skrince |
|
|
465 |
last_log_odo=0; // Posledni zaznam odometrie do logu |
|
|
466 |
|
|
|
467 |
ReadBlackBox(); // Vycteni zaznamu z Black Boxu |
|
|
468 |
|
|
|
469 |
odo_problem1=ODODO_PROBLEM1-BRZDNA_DRAHA; |
|
|
470 |
odo_problem2=ODODO_PROBLEM2-BRZDNA_DRAHA; |
|
|
471 |
odo_problem3=ODODO_PROBLEM3-BRZDNA_DRAHA; |
|
|
472 |
|
|
|
473 |
// ........................... Hlavni smycka ................................ |
|
|
474 |
while(true) |
|
|
475 |
{ |
|
|
476 |
int8 pom; |
|
|
477 |
int8 n; |
|
|
478 |
int8 gap; |
|
|
479 |
int16 ododo; |
|
|
480 |
|
|
|
481 |
gap=0; // Vynuluj pocitadlo preruseni |
|
|
482 |
|
|
|
483 |
next_snap: |
|
|
484 |
|
|
|
485 |
pom=0; |
|
|
486 |
disable_interrupts(GLOBAL); //----------------------- Critical Section |
|
|
487 |
while(input(HREF)); // Preskoc 1. radku |
|
|
488 |
while(!input(HREF)); // Cekej nez se zacnou posilat pixely z 2. radky |
|
|
489 |
set_timer0(0); // Vynuluj pocitadlo casu |
|
|
490 |
while(input(HREF)) // Po dobu vysilani radky cekej na hranu W/B |
|
|
491 |
{ |
|
|
492 |
// !!!!Dodelat rozpoznani cerne cary napric pro zastaveni ? |
|
|
493 |
if(!input(PIX)) // Pokud se X-krat za sebou precetla CERNA |
|
|
494 |
if(!input(PIX)) |
|
|
495 |
// if(!input(PIX)) |
|
|
496 |
{ |
|
|
497 |
pom=get_timer0(); // Precti cas z citace casu hrany |
|
|
498 |
break; |
|
|
499 |
}; |
|
|
500 |
}; |
|
|
501 |
while(input(HREF)); // Pockej na shozeni signalu HREF |
|
|
502 |
|
|
|
503 |
if((pom<CASMAX) && (pom>CASMIN)) cas=pom; // Orizni konce radku |
|
|
504 |
// Na konci obrazovaho radku to blbne. Jednak chyba od apertury |
|
|
505 |
// a vubec to nejak na kraji nefunguje. |
|
|
506 |
|
|
|
507 |
output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo |
|
|
508 |
delay_us(1000); |
|
|
509 |
delay_us(stred); |
|
|
510 |
delay_us(stred); |
|
|
511 |
delay_us(stred); |
|
|
512 |
delay_us(cas); |
|
|
513 |
delay_us(cas); |
|
|
514 |
output_low(SERVO); |
|
|
515 |
|
|
|
516 |
// Elektronicky diferencial 1. cast |
|
|
517 |
if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace |
|
|
518 |
if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;}; // pro rizeni rychlosti motoru |
|
|
519 |
if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92 |
|
|
520 |
|
|
|
521 |
enable_interrupts(GLOBAL); //----------------------- End Critical Section |
|
|
522 |
|
|
|
523 |
if(pom==0) // Kamera nevidi caru |
|
|
524 |
{ |
|
|
525 |
if((cas>(CASMIN+15))&&(cas<(CASMAX-15))) // Nebyla minule cara moc u kraje? |
|
|
526 |
{ |
|
|
527 |
gap++; |
|
|
528 |
if(gap>=3) // Trva preruseni cary alespon 2 snimky? |
|
|
529 |
{ |
|
|
530 |
cas=CASAVR-CASMIN; |
|
|
531 |
// disp(0xAA); |
|
|
532 |
} |
|
|
533 |
} |
|
|
534 |
} |
|
|
535 |
else |
|
|
536 |
{ |
|
|
537 |
gap=0; |
|
|
538 |
}; |
|
|
539 |
|
|
|
540 |
|
|
|
541 |
/* |
|
|
542 |
if(pom==0) // Kamera nevidi caru, poznamenej to do logu |
|
|
543 |
{ |
|
|
544 |
if((cas>(CASMIN+30))&&(cas<(CASMAX-30))) // Nebyla minule cara moc u kraje? |
|
|
545 |
if(last_log_odo<get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu |
|
|
546 |
{ |
|
|
547 |
gap++; |
|
|
548 |
} |
|
|
549 |
} |
|
|
550 |
else |
|
|
551 |
{ |
|
|
552 |
if(gap>=4) // Trva preruseni cary alespon 2 snimky? |
|
|
553 |
{ |
|
|
554 |
LogLog(gap,8); // Dalsi mereni nejdrive po ujeti 24 cm |
|
|
555 |
rr=rrold; // Preruseni cary skoncilo, znovu jed Rozumnou Rychlosti |
|
|
556 |
cas=CASAVR-CASMIN; |
|
|
557 |
disp(0xAA); |
|
|
558 |
} |
|
|
559 |
gap=0; |
|
|
560 |
}; |
|
|
561 |
|
|
|
562 |
if(!input(PROXIMITY) && ((stav==jizda)||(stav==cihla))) // Tunel |
|
|
563 |
{ |
|
|
564 |
if(last_log_odo<get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu |
|
|
565 |
{ |
|
|
566 |
LogLog(0xDD,16); // Priznak tunelu; dalsi mereni nejdrive po ujeti 48 cm |
|
|
567 |
rr=rrold; // Vjeli jsme do tunelu, znovu jed rychle |
|
|
568 |
} |
|
|
569 |
}; |
|
|
570 |
*/ |
|
|
571 |
|
|
|
572 |
//ODODO |
|
|
573 |
ododo=get_timer1(); |
|
|
574 |
if((ododo>odo_preruseni)&&(ododo<(odo_preruseni+8))) rr=RR_PRERUSENI; |
|
|
575 |
if((ododo>odo_cihla)&&(ododo<(odo_cihla+8))) rr=RR_PRERUSENI; |
|
|
576 |
if((ododo>odo_tunel)&&(ododo<(odo_tunel+8))) rr=RR_PRERUSENI; |
|
|
577 |
if((ododo>odo_problem1)&&(ododo<(odo_problem1+8))) rr=RR_PRERUSENI; |
|
|
578 |
if((ododo>odo_problem2)&&(ododo<(odo_problem2+8))) rr=RR_PRERUSENI; |
|
|
579 |
if((ododo>odo_problem3)&&(ododo<(odo_problem3+8))) rr=RR_PRERUSENI; |
|
|
580 |
|
|
|
581 |
// Elektronicky diferencial 2. cast |
|
|
582 |
if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN); // Neco jako nasobeni |
|
|
583 |
if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN); // rozsah 1 az 92 pro rr=0 // rozsah 1 az 154 pro rr=63 |
|
|
584 |
|
|
|
585 |
//!!! pro zatuhle prevodovky |
|
|
586 |
// r1<<=1; // Rychlost je dvojnasobna |
|
|
587 |
// r2<<=1; // Rozsah 2 az 184 pro rr=0 |
|
|
588 |
|
|
|
589 |
if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle)) // Jizda |
|
|
590 |
{ |
|
|
591 |
set_pwm1_duty(r1); |
|
|
592 |
set_pwm2_duty(r2); |
|
|
593 |
} |
|
|
594 |
else |
|
|
595 |
{ |
|
|
596 |
set_pwm1_duty(0); // Zastaveni |
|
|
597 |
set_pwm2_duty(0); |
|
|
598 |
}; |
|
|
599 |
|
|
|
600 |
if((stav==rozjezd)&&(get_timer1()>10)) // musi ujet alespon 31cm |
|
|
601 |
{ |
|
|
602 |
ext_int_edge(H_TO_L); // Nastav podminky preruseni od cihly |
|
|
603 |
INT0IF=0; // Zruseni predesle udalosti od startera |
|
|
604 |
enable_interrupts(INT_EXT); |
|
|
605 |
enable_interrupts(GLOBAL); |
|
|
606 |
stav=jizda; |
|
|
607 |
}; |
|
|
608 |
|
|
|
609 |
if(stav==start) // Snimkuje, toci servem a ceka na start |
|
|
610 |
{ |
|
|
611 |
set_adc_channel(MODRA); // Kroutitko na vystredeni predniho kolecka |
|
|
612 |
Delay_ms(1); |
|
|
613 |
stred=read_adc(); |
|
|
614 |
if(!input(PROXIMITY)) |
|
|
615 |
{ |
|
|
616 |
disp(0x80); |
|
|
617 |
while(input(PROXIMITY)); // Cekej, dokud starter neda ruku pryc |
|
|
618 |
set_timer1(0); // Vynuluj citac odometrie |
|
|
619 |
set_pwm1_duty(255); // Rychly rozjezd !!! Zkontrolovat na oscyloskopu |
|
|
620 |
set_pwm2_duty(255); |
|
|
621 |
disp(0x01); |
|
|
622 |
while(get_timer1()<=4) // Ujed alespon 12cm |
|
|
623 |
{ |
|
|
624 |
set_adc_channel(LMAX); // Levy UV sensor |
|
|
625 |
delay_us(40); |
|
|
626 |
if(read_adc()<THR) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo |
|
|
627 |
set_adc_channel(RMAX); // Pravy UV sensor |
|
|
628 |
delay_us(40); |
|
|
629 |
if(read_adc()<THR) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo |
|
|
630 |
cas=CASAVR-CASMIN; // Cara je rovne |
|
|
631 |
}; |
|
|
632 |
stav=rozjezd; |
|
|
633 |
}; |
|
|
634 |
} |
|
|
635 |
|
|
|
636 |
pom=0x80; // Zobrazeni pozice cary na displayi |
|
|
637 |
for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1; |
|
|
638 |
disp(pom); |
|
|
639 |
|
|
|
640 |
while(true) // Ve zbytku casu snimku cti krajni UV senzory a naraznik |
|
|
641 |
{ |
|
|
642 |
if(BUMPER) // Sakra, do neceho jsme narazili a nevideli jsme to! |
|
|
643 |
{ |
|
|
644 |
bum(); |
|
|
645 |
SaveLog(); // Zapis Black Boxu do EEPROM |
|
|
646 |
delay_ms(TUHOS); //!!! Zatuhle prevodovky |
|
|
647 |
set_pwm1_duty(200); // pomalu vpred |
|
|
648 |
set_pwm2_duty(200); |
|
|
649 |
output_low(MOT_L); |
|
|
650 |
output_low(MOT_R); |
|
|
651 |
cas=CASAVR-CASMIN; |
|
|
652 |
}; |
|
|
653 |
set_adc_channel(LMAX); // Levy UV sensor |
|
|
654 |
for(n=0;n<20;n++) if(input(HREF)) goto next_snap; |
|
|
655 |
if(read_adc()<THR) cas=CASMIN; |
|
|
656 |
set_adc_channel(RMAX); // Pravy UV sensor |
|
|
657 |
for(n=0;n<20;n++) if(input(HREF)) goto next_snap; |
|
|
658 |
if(read_adc()<THR) cas=CASMAX; |
|
|
659 |
}; |
|
|
660 |
} |
|
|
661 |
} |