Rev Author Line No. Line
3363 cizelu 1 #include "main.h"
2  
3 // NEPOUZIVAT PINY B6 A B7, JSOU VYHRAZENY PRO SERIOVOU KOMUNIKACI
4 // BAUD RATE = 9600
5 // ========================== PRIPRAVA DAT A VYSTUPU ===========================
6 // pomocne konstanty
7 #define LEFT 0
8 #define RIGHT 1
9 #define DET_EN 0 // povoluje nebo zakazuje vyhodnoceni SHARP
10  
11 // regulator
12 #define CONP 2 // konstanta pro proporcionalni regulator (2)
13 #define CONI 0 // konstanta pro integracni regulator *0,01 (45)
14 #define COND 0 // konstanta pro derivacni regulator *0,01 (20)
15  
16 #define SPD_LO 220 // zaklad pro vypocet rychlosti pomalejsiho motoru (130)
17 #define SPD_HI 220 // zaklad pro vypocetrychlosti rychlejsiho motoru (155)
18 #define SPD_MAX 220 // rychlost po rovince (240)
19  
20 int8 reg_out;
21 int8 err1; // odchylka prvni hodnoty
22 int8 err2;
23 int8 err3;
24 int8 err4;
25 int8 err5; // odchylka posledni hodnoty
26 int8 errp; // prumer chyb
27  
28 // mezera
29 #define SPACE 8 // jak dlouho robot smi nic nevidet (8)
30 #define CONT 25 // kontrast, kdy nic nevidime
31  
32 // univerzalni LED diody
33 #define LED1 PIN_E1
34 #define LED2 PIN_E0
35  
36 int16 blink;
37  
38 // piezo pipak
39 #DEFINE SOUND_HI PIN_B4
40 #DEFINE SOUND_LO PIN_B5
41  
42 // radkovy senzor
43 #define SDIN PIN_D4 // seriovy vstup
44 #define SDOUT input(PIN_C5) // seriovy vystup
45 #define SCLK PIN_D5 // takt
46  
47 // pro komunikaci s OLSA, prvni se posila LSB
48 int MAIN_RESET[8]={1,1,0,1,1,0,0,0}; // hlavni reset 0x1B
49 int SET_MODE_RG[8]={1,1,1,1,1,0,1,0}; // zapis do MODE registru 0x5F
50 int CLEAR_MODE_RG[8]={0,0,0,0,0,0,0,0}; // nulovani MODE registru 0x00
51  
52 int LEFT_OFFSET[8]={0,0,0,0,0,0,1,0}; // offset leveho segmentu senzoru 0x40
53 int MID_OFFSET[8]={0,1,0,0,0,0,1,0}; // offset prostredniho segmentu senzoru 0x42
54 int RIGHT_OFFSET[8]={0,0,1,0,0,0,1,0}; // offset praveho segmentu senzoru 0x44
55 int OFFSET[8]={1,0,0,0,0,0,0,1}; // minus jedna - pouzit pro vsechny segmenty 0x81
56  
57 int LEFT_GAIN[8]={1,0,0,0,0,0,1,0}; // zisk leveho segmentu 0x41
58 int MID_GAIN[8]={1,1,0,0,0,0,1,0}; // zisk leveho segmentu 0x43
59 int RIGHT_GAIN[8]={1,0,1,0,0,0,1,0}; // zisk leveho segmentu 0x45
60 int GAIN[8]={1,0,1,0,0,0,0,0}; // zisk = 5 - pouzit pro vsechny segmenty 0x5
61  
62 int START_INT[8]={0,0,0,1,0,0,0,0}; // zacatek integrace 0x08
63 int STOP_INT[8]={0,0,0,0,1,0,0,0}; // konec integrace 0x10
64 int READOUT[8]={0,1,0,0,0,0,0,0}; // cteni senzoru 0x02
65  
66 int olsa_lseg[51]={0}; // leva cast radky (pixely 0 - 50)
67 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101)
68 int8 *lp; // ukazatel pro levou polovinu radky
69 int8 *rp; // ukazatel pro levou polovinu radky
70  
71 int8 contrast; // rozdil mezi nejsvetlejsim a nejtmavsi mistem
72 int8 position; // ulozeni pozice cary
73 int8 old_position; // ulozeni predchozi pozice cary
74 int1 line_sector; // cara je vlevo/vpravo
75 int8 gap; // pocita, jak dlouho neni videt cara
76  
77 // ================================= NARAZNIK ==================================
78 #define BUMPL input(PIN_D6)
79 #define BUMPR input(PIN_D7)
80  
81 // ============================= NOUZOVE SENZORY ===============================
82 #define LINEL 0 // analogovy kanal pro levy senzor
83 #define LINER 1 // analogovy kanal pro pravy senzor
84 #define WHITE 30 // rozhodovaci uroven pro nouzove senzory
85  
86 int8 line_l; // uklada hodnotu leveho senzoru
87 int8 line_r; // uklada hodnotu praveho senzoru
88  
89 // ================================ DALKOMER ===================================
90 #define SHARP 2 // analogovy kanal pro SHARP
91 #define PROBLEM 55 // rozhodovaci uroven, kdy hrozi prekazka
92 #define BLOCK 65 // rozhodovaci uroven, kdy je jiste prekazka
93 #define DANGER 10 // pocita, jak dlouho je detekovan problem
94  
95 int8 p_count;
96 int8 sharp_lev; // uklada hodnotu sharp
97  
98 // ================================== MOTORY ===================================
99 #define LMF PIN_D0
100 #define LMB PIN_D1
101 #define RMF PIN_D2
102 #define RMB PIN_D3
103  
104 int8 lm_speed; // rychlost leveho motoru
105 int8 rm_speed; // rychlost praveho motoru
106  
107 // =============================== PODPROGRAMY =================================
108  
109 // ================================= OLSA01A ===================================
110  
111 void olsa_pulses(int count) // vytvori impulzy pro ridici logiku
112 {
113 int8 ct;
114 for(ct=0;ct<=count;ct++)
115 {
116 output_high(SCLK);
117 output_low(SCLK);
118 }
119 }
120  
121 void olsa_pulse() // vytvori jeden impulz
122 {
123 output_high(SCLK);
124 output_low(SCLK);
125 }
126  
127 void olsa_send(int8 info[8]) // USART komunikace s modulem OLSA01A - poslani zpravy
128 {
129 int *ip; // ukazatel na pole s informaci
130 int8 i; // pomocna promenna pro nastaveni 0 nebo 1 na SDIN
131 output_low(SDIN); // start bit
132 olsa_pulse();
133 for(ip=0;ip<8;ip++) // predani informace - 8 bit, LSB prvni > MSB posledni
134 {
135 i=info[ip]; // ziskani hodnoty z pole
136 if(i==1) // vyhodnoceni obsahu informace - nastav 1
137 {
138 output_high(SDIN);
139 }
140 else // vyhodnoceni obsahu informace - nastav 0
141 {
142 output_low(SDIN);
143 }
144 olsa_pulse();
145 }
146 output_high(SDIN); // stop bit
147 olsa_pulse();
148 }
149  
150 void olsa_reset() // hlavni RESET - provadi se po zapnuti
151 {
152 output_low(SDIN);
153 output_low(SCLK);
154 olsa_pulses(30); // reset radkoveho senzoru
155 output_high(SDIN);
156 olsa_pulses(10); // start bit - synchronizace
157 olsa_send(MAIN_RESET);
158 olsa_pulses(5);
159 olsa_send(SET_MODE_RG);
160 olsa_send(CLEAR_MODE_RG);
161 }
162  
163 void olsa_setup() // kompletni nastaveni, provadi se po resetu
164 {
165 olsa_send(LEFT_OFFSET); // nastaveni leveho segmentu (offset a zisk)
166 olsa_send(OFFSET);
167 olsa_send(LEFT_GAIN);
168 olsa_send(GAIN);
169 olsa_send(MID_OFFSET); // nastaveni prostredniho segmentu (offset a zisk)
170 olsa_send(OFFSET);
171 olsa_send(MID_GAIN);
172 olsa_send(GAIN);
173 olsa_send(RIGHT_OFFSET); // nastaveni praveho segmentu (offset a zisk)
174 olsa_send(OFFSET);
175 olsa_send(RIGHT_GAIN);
176 olsa_send(GAIN);
177 }
178  
179 void olsa_integration() // snimani pixelu
180 {
181 olsa_send(START_INT); // zacatek integrace senzoru
182 olsa_pulses(22);
183 olsa_send(STOP_INT); // konec integrace senzoru
184 olsa_pulses(5);
185 }
186  
187 void read_olsa()
188 {
189 int8 cpixel; // pocet prectenych pixelu
190 int8 cbit; // pocet prectenych bitu
191 int8 pixel; // hodnota precteneho pixelu
192 cpixel=0;
193 lp=0;
194 rp=0;
195 olsa_integration();
196 olsa_send(READOUT);
197 do // precte 102 pixelu
198 {
199 if(!SDOUT) // zacatek prenosu - zachycen start bit
200 {
201 pixel=0;
202 for(cbit=0;cbit<8;cbit++) // cte jednotlive bity (8 bitu - 0 az 7)
203 {
204 olsa_pulse(); // impulz pro generovani dalsiho bitu
205  
206 if(SDOUT) // zachycena 1
207 {
208 bit_set(pixel,cbit); // zapise do bitu (dano cbit) bytu (pixelu) prislusnou hodnotu
209 }
210 }
211 olsa_pulse(); // generuje stop bit
212 if(cpixel<52) // ulozeni do pole
213 {
214 olsa_lseg[lp]=pixel; // leva polovina radky - leve pole
215 lp++;
216 }
217 else
218 {
219 olsa_rseg[rp]=pixel; // prava polovina cary - prave pole
220 rp++;
221 }
222 cpixel++;
223 }
224 else
225 {
226 olsa_pulse(); // generuje start bit, nebyl-li poslan
227 }
228 }
229 while(cpixel<102); // precte 102 pixelu
230 }
231  
232 void olsa_position() // vyhodnoti pozici cary
233 {
234 int8 searchp; // ukazatel na pole
235 int8 dark; // nejtmavsi pixel
236 int8 bright; // nejsvetlejsi pixel
237 dark=0xff;
238 bright=0x00;
239 for(searchp=0;searchp<51;searchp++) // prohlizi levou cast radky
240 {
241 if(olsa_lseg[searchp]<dark) // porovna pixel s doposud nejtmavsim
242 {
243 dark=olsa_lseg[searchp]; // ulozi nejtmavsi pixel
244 position=searchp; // ulozi polohu nejtmavsiho pixelu
245 }
246 if(olsa_lseg[searchp]>bright)
247 {
248 bright=olsa_lseg[searchp]; // ulozi nejsvetlejsi pixel
249 }
250 }
251 for(searchp=0;searchp<49;searchp++) // prohlizi levou cast radky
252 {
253 if(olsa_rseg[searchp]<dark) // porovna pixel s doposud nejtmavsim
254 {
255 dark=olsa_rseg[searchp]; // ulozi nejtmavsi pixel
256 position=(searchp+51); // ulozi polohu nejtmavsiho pixelu
257 }
258 if(olsa_rseg[searchp]>bright)
259 {
260 bright=olsa_rseg[searchp]; // ulozi nejsvetlejsi pixel
261 }
262 }
263 contrast=(bright-dark);
264 if(contrast<CONT)
265 {
266 position=0;
267 }
268 }
269  
270 // ============================ ZACHRANNE SENZORY ==============================
271  
272 void read_blue_sensors() // cteni nouzovych senzoru
273 {
274 set_adc_channel(LINEL); // cti levy nouzovy senzor
275 delay_us(10);
276 line_l=read_adc();
277 set_adc_channel(LINER); // cti pravy nouzovy senzor
278 delay_us(10);
279 line_r=read_adc();
280 }
281  
282 // ================================= DALKOMER =================================
283  
284 void read_sharp() // cteni z dalkomeru
285 {
286 set_adc_channel(SHARP); // cteni z dalkomeru
287 delay_us(10);
288 sharp_lev=read_adc(); // ulozeni hodnoty
289 }
290  
291 // ================================== PIPAK ====================================
292  
293 void beep(int16 period,int16 length)
294 {
295 int16 bp; // promenna pro nastaveni delky
296 for(bp=length;bp>0;bp--) // prepina vystupy tolikrat, jakou jsme zadali delku
297 {
298 output_high(SOUND_HI);
299 output_low(SOUND_LO);
300 delay_us(period);
301 output_high(SOUND_LO);
302 output_low(SOUND_HI);
303 delay_us(period);
304 }
305 }
306  
307 // ================================= REGULATOR =================================
308  
309 void calc_error()
310 {
311 err1=err2; // ulozeni chyb
312 err2=err3;
313 err3=err4;
314 err4=err5;
315 if(position<50) // ulozeni a vypocet aktualni absolutni chyby
316 {
317 err5=(50-position); // pokud je cara vlevo
318 }
319 else
320 {
321 err5=(position-50); // pokud je cara vpravo
322 }
323 errp=((err1+err2+err3+err4+err5)/5); // vypocet chyby pro integracni regulator
324 }
325 void calc_regulator()
326 {
327 int8 p_reg;
328 int8 i_reg;
329 int8 d_reg;
330 p_reg=(CONP*err5); // vypocet proporcionalni slozky
331 i_reg=(CONI*(errp/100)); // vypocet integracni slozky
332 if(position>old_position) // vypocet derivacni slozky
333 {
334 d_reg=(COND*((position-old_position)/100)); // pokud je aktualni pozice vetsi nez predesla
335 }
336 else
337 {
338 d_reg=(COND*((old_position-position)/100)); // pokud je aktualni pozice mensi nez predesla
339 }
340 reg_out=(p_reg+i_reg+d_reg); // vypocet celeho regulatoru
341 }
342  
343 // ================================== MOTORY ===================================
344  
345 void l_motor_fwd(int8 speedl) // levy motor dopredu
346 {
347 output_high(LMF);
348 output_low(LMB);
349 set_pwm2_duty(speedl);
350 }
351  
352 void l_motor_bwd(int8 speedl) // levy motor dozadu
353 {
354 output_high(LMB);
355 output_low(LMF);
356 set_pwm2_duty(speedl);
357 }
358  
359 void r_motor_fwd(int8 speedr) // pravy motor dopredu
360 {
361 output_high(RMF);
362 output_low(RMB);
363 set_pwm1_duty(speedr);
364 }
365  
366 void r_motor_bwd(int8 speedr) // pravy motor dozadu
367 {
368 output_high(RMB);
369 output_low(RMF);
370 set_pwm1_duty(speedr);
371 }
372  
373 void l_motor_off() // levy motor vypnut
374 {
375 output_low(LMF);
376 output_low(LMB);
377 set_pwm2_duty(0);
378 }
379  
380 void r_motor_off() // pravy motor vypnut
381 {
382 output_low(RMF);
383 output_low(RMB);
384 set_pwm1_duty(0);
385 }
386  
387 void motor_test() // TEST MOTORU
388 {
389 int8 i;
390 beep(100,200);
391 printf("TEST MOTORU\r\n");
392 delay_ms(1000);
393 printf("LEVY MOTOR DOPREDU\r\n");
394 delay_ms(1000);
395 for(i=0;i<255;i++) // levy motor dopredu - zrychluje
396 {
397 l_motor_fwd(i);
398 printf("RYCHLOST: %u\r\n",i);
399 delay_ms(5);
400 }
401 for(i=255;i>0;i--) // levy motor dopredu - zpomaluje
402 {
403 l_motor_fwd(i);
404 printf("RYCHLOST: %u\r\n",i);
405 delay_ms(5);
406 }
407 printf("LEVY MOTOR DOZADU\r\n"); // levy motor dozadu - zrychluje
408 delay_ms(1000);
409 for(i=0;i<255;i++)
410 {
411 l_motor_bwd(i);
412 printf("RYCHLOST: %u\r\n",i);
413 delay_ms(5);
414 }
415 for(i=255;i>0;i--) // levy motor dozadu - zpomaluje
416 {
417 l_motor_bwd(i);
418 printf("RYCHLOST: %u\r\n",i);
419 delay_ms(5);
420 }
421 printf("PRAVY MOTOR DOPREDU\r\n");
422 delay_ms(1000);
423 for(i=0;i<255;i++) // pravy motor dopredu - zrychluje
424 {
425 r_motor_fwd(i);
426 printf("RYCHLOST: %u\r\n",i);
427 delay_ms(5);
428 }
429 for(i=255;i>0;i--) // pravy motor dopredu - zpomaluje
430 {
431 r_motor_fwd(i);
432 printf("RYCHLOST: %u\r\n",i);
433 delay_ms(5);
434 }
435 printf("PRAVY MOTOR DOZADU\r\n");
436 delay_ms(1000);
437 for(i=0;i<255;i++) // pravy motor dozadu - zrychluje
438 {
439 r_motor_bwd(i);
440 printf("RYCHLOST: %u\r\n",i);
441 delay_ms(5);
442 }
443 for(i=255;i>0;i--) // pravy motor dozadu - zpomaluje
444 {
445 r_motor_bwd(i);
446 printf("RYCHLOST: %u\r\n",i);
447 delay_ms(5);
448 }
449 l_motor_off(); // po ukonceni testu vypnout motory
450 r_motor_off();
451 printf("KONEC TESTU MOTORU\r\n");
452 delay_ms(1000);
453 }
454  
455 // ================================ OBJETI CIHLY ===============================
456  
457 void detour() // po detekci prekazky zacne objizdeni
458 {
459 l_motor_bwd(200); // zatoc doleva
460 r_motor_fwd(200);
461 delay_ms(400);
462 l_motor_fwd(200); // jed rovne
463 delay_ms(800);
464 r_motor_bwd(200); // zatoc doprava
465 delay_ms(200);
466 r_motor_fwd(200); // jed rovne
467 delay_ms(1000);
468 r_motor_bwd(200); // zatoc doprava
469 delay_ms(100);
470 l_motor_fwd(180);
471 r_motor_fwd(255); // jed rovne
472 delay_ms(200);
473 position=40;
474 }
475 // ================================ DIAGNOSTIKA ================================
476  
477 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
478 {
479 read_blue_sensors(); // cteni nouzovych senzoru
480 read_sharp(); // cteni dalkomeru
481 read_olsa(); // cteni z optickeho radkoveho senzoru
482 olsa_position();
483 printf("LEVA: %u \t",line_l); // tiskne z leveho senzoru
484 printf("PRAVA: %u \t",line_r); // tiskne z praveho senzoru
485 printf("SHARP: %u \t",sharp_lev); // tiskne z dalkomeru
486 printf("POLOHA: %u\t",position); // tiskne pozici OLSA
487 printf("KONTRAST: %u \t", contrast); // tiskne kontrast z OLSA
488 printf("L_NARAZ: %u \t",BUMPL); // leve tlacitko narazniku
489 printf("P_NARAZ: %u \r\n",BUMPR); // prave tlacitko narazniku
490 if(BUMPL&&BUMPR) // po zmacknuti stran narazniku spusti test motoru
491 {
492 beep(100,1000);
493 printf("Levy naraznik - test OLSA\r\n");
494 printf("Pravy naraznik - test motoru\r\n");
495 delay_ms(500);
496 while(true)
497 {
498 if(BUMPR)
499 {
500 beep(100,500); // pipni pri startu
501 motor_test();
502 }
503 if(BUMPL)
504 {
505 beep(100,500);
506 printf("TEST OLSA\r\n");
507 while(true)
508 {
509 int8 tisk;
510 int8 *tiskp;
511 read_olsa();
512 printf("cteni\r\n"); // po precteni vsech pixelu odradkuje
513 for(tiskp=0;tiskp<52;tiskp++) // tisk leve casti radky
514 {
515 tisk=olsa_lseg[tiskp];
516 printf("%x ",tisk);
517 }
518 for(tiskp=0;tiskp<52;tiskp++) // tisk prave casti radky
519 {
520 tisk=olsa_rseg[tiskp];
521 printf("%x ",tisk);
522 }
523 }
524 }
525 }
526 }
527 }
528  
529 // ============================== HLAVNI SMYCKA ================================
530  
531 void main()
532 {
533 printf("POWER ON \r\n");
534 // NASTAVENI > provede se pouze pri zapnuti
535 setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik
536 setup_adc_ports(ALL_ANALOG); // aktivni vsechny analogove vstupy
537 setup_spi(SPI_SS_DISABLED);
538 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
539 setup_timer_1(T1_DISABLED);
540 setup_timer_2(T2_DIV_BY_16,255,1); // casovac pro PWM
541 setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2
542 setup_ccp2(CCP_PWM); // povolí PWM na pinu RC1
543 setup_comparator(NC_NC_NC_NC);
544 setup_vref(FALSE);
545 l_motor_off(); // vypne levy motor
546 r_motor_off(); // vypne pravy motor
547 olsa_reset();
548 olsa_setup();
549 beep(350,300); // pipni pri startu
550 printf("OK! \r\n");
551 delay_ms(500);
552 printf("VYBRAT MOD... \r\n");
553 // ============================ HLAVNI CAST PROGRAMU ===========================
554 while(true)
555 {
556 if(blink<4000)
557 {
558 output_low(LED1);
559 output_high(LED2);
560 }
561 else
562 {
563 output_low(LED2);
564 output_high(LED1);
565 }
566 if (blink==8000)
567 {
568 blink=0;
569 }
570 blink++;
571 // ================================ DIAGNOSTIKA ================================
572 if(BUMPL)
573 {
574 output_low(LED1);
575 output_high(LED2);
576 beep(200,500);
577 while(true)
578 {
579 diag();
580 }
581 }
582 // =============================== SLEDOVANI CARY ==============================
583 if(BUMPR) // spusteni hledani pravym naraznikem
584 {
585 output_low(LED2);
586 output_high(LED1);
587 beep(300,500);
588 while(true)
589 {
590 old_position=position; // zaznamena predhozi polohu cary
591 read_olsa(); // precte a ulozi hodnoty z olsa
592 olsa_position(); // vyhodnoti pozici cary
593 read_blue_sensors(); // cte nouzove senzory
594 read_sharp(); // cte dalkomer
595 if(position==0) // pokud neni videt cara
596 {
597 position=old_position; // nastav predchozi pozici
598 gap++; // pocita, jak dlouho neni videt cara
599 }
600 else // pokud je videt
601 {
602 gap=0; // gap je roven nule
603 }
604 if(gap>SPACE)
605 {
606 if(line_l<WHITE) // cara videna levym modrym senzorem
607 {
608 position=1;
609 line_sector=LEFT;
610 }
611 if(line_r<WHITE) // cara videna pravym modrym senzorem
612 {
613 position=100;
614 line_sector=RIGHT;
615 }
616 }
617 calc_error();
618 calc_regulator();
619 //printf("regulator: %u\r\n",reg_out);
620 if(position<47) // prepocet regulatoru pro motory, pokud je cara vlevo
621 {
622 lm_speed=SPD_LO-(2*reg_out);
623 rm_speed=SPD_HI;
624 line_sector=LEFT;
625 }
626 if((position>46)&&(position<54)) // nastaveni rychlosti, pokud je cara uprostred
627 {
628 lm_speed=SPD_MAX;
629 rm_speed=SPD_MAX;
630 }
631 if(position>53) // prepocet regulatoru pro motory, pokud je cara vpravo
632 {
633 lm_speed=SPD_HI;
634 rm_speed=SPD_LO-(2*reg_out);
635 line_sector=RIGHT;
636 }
637 if((sharp_lev>PROBLEM)&&(DET_EN)) // zachycen odraz na dalkomeru
638 {
639 p_count++; // pocita, jak dlouho je videt
640 output_low(LED1);
641 if(p_count>DANGER) // pokud je odraz videt nebezpecne dlouho, zpomali
642 {
643 lm_speed=(lm_speed/2);
644 rm_speed=(rm_speed/2);
645 }
646 }
647 else // pokud jiz neni detekoven odraz, vynuluj pocitadlo
648 {
649 p_count=0;
650 output_high(LED1);
651 }
652 if((sharp_lev>BLOCK)&&(DET_EN)) // odraz zachycen nebezpecne blizko
653 {
654 l_motor_off();
655 r_motor_off();
656 beep(100,200);
657 read_sharp();
658 if(sharp_lev>BLOCK) // pokud je porad videt prekazka
659 {
660 detour(); // spust objizdeni
661 }
662 }
663 l_motor_fwd(lm_speed);
664 r_motor_fwd(rm_speed);
665 }
666 }
667 }
668 }
669  
670