Rev Author Line No. Line
799 cizelu 1 #include "main.h"
842 cizelu 2  
708 cizelu 3 // NEPOUZIVAT PINY B6 A B7, JSOU VYHRAZENY PRO SERIOVOU KOMUNIKACI
842 cizelu 4 // BAUD RATE = 9600
5  
865 cizelu 6 // pomocne promenne
7 #define LEFT 0
8 #define RIGHT 1
842 cizelu 9 // univerzalni LED diody
830 cizelu 10 #define LED1 PIN_E0
11 #define LED2 PIN_E1
700 cizelu 12  
842 cizelu 13 // piezo pipak
716 cizelu 14 #DEFINE SOUND_HI PIN_B4
15 #DEFINE SOUND_LO PIN_B5
700 cizelu 16  
842 cizelu 17 // radkovy senzor
745 cizelu 18 #define SDIN PIN_D4 // seriovy vstup
830 cizelu 19 #define SDOUT input(PIN_C5) // seriovy vystup
745 cizelu 20 #define SCLK PIN_D5 // takt
727 cizelu 21  
865 cizelu 22 #define LINE_PX 5 // pocet pixelu pro jiste urceni cary
868 cizelu 23 #define OLSA_LEV 0x60 // rozhodovaci uroven (cca 10 odpovida cerne)
840 cizelu 24  
842 cizelu 25 // pro komunikaci s OLSA, prvni se posila LSB
829 cizelu 26 int main_reset[8]={1,1,0,1,1,0,0,0}; // hlavni reset 0x1B
27 int set_mode_rg[8]={1,1,1,1,1,0,1,0}; // zapis do MODE registru 0x5F
28 int clear_mode_rg[8]={0,0,0,0,0,0,0,0}; // nulovani MODE registru 0x00
730 cizelu 29  
829 cizelu 30 int left_offset[8]={0,0,0,0,0,0,1,0}; // offset leveho segmentu senzoru 0x40
31 int mid_offset[8]={0,1,0,0,0,0,1,0}; // offset prostredniho segmentu senzoru 0x42
32 int right_offset[8]={0,0,1,0,0,0,1,0}; // offset praveho segmentu senzoru 0x44
33 int offset[8]={1,0,0,0,0,0,0,1}; // minus jedna - pouzit pro vsechny segmenty 0x81
741 cizelu 34  
829 cizelu 35 int left_gain[8]={1,0,0,0,0,0,1,0}; // zisk leveho segmentu 0x41
36 int mid_gain[8]={1,1,0,0,0,0,1,0}; // zisk leveho segmentu 0x43
37 int right_gain[8]={1,0,1,0,0,0,1,0}; // zisk leveho segmentu 0x45
38 int gain[8]={1,0,1,0,0,0,0,0}; // zisk = 5 - pouzit pro vsechny segmenty 0x5
771 kaklik 39  
829 cizelu 40 int start_int[8]={0,0,0,1,0,0,0,0}; // zacatek integrace 0x08
41 int stop_int[8]={0,0,0,0,1,0,0,0}; // konec integrace 0x10
42 int readout[8]={0,1,0,0,0,0,0,0}; // cteni senzoru 0x02
789 cizelu 43  
829 cizelu 44 int olsa_lseg[51]={0}; // leva cast radky (pixely 0 - 50)
45 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101)
837 cizelu 46 int8 *lp; // ukazatel pro levou polovinu radky
47 int8 *rp; // ukazatel pro levou polovinu radky
812 cizelu 48  
842 cizelu 49 //naraznik
708 cizelu 50 #define BUMPL input(PIN_D6)
51 #define BUMPR input(PIN_D7)
700 cizelu 52  
842 cizelu 53 //nouzove senzory
708 cizelu 54 #define LINEL 0
55 #define LINER 1
842 cizelu 56 #define BLUE_LEV 200 // rozhodovaci uroven (zmereno 0 - cerna, 255 cerna)
789 cizelu 57 int8 line_l;
58 int8 line_r;
59 int1 line_position;
700 cizelu 60  
842 cizelu 61 // motory
708 cizelu 62 #define LMF PIN_D0
63 #define LMB PIN_D1
64 #define RMF PIN_D2
65 #define RMB PIN_D3
842 cizelu 66  
67 int8 lm_speed;
790 cizelu 68 int8 rm_speed;
69  
700 cizelu 70 //PODPROGRAMY
71 //SENZORY
730 cizelu 72 //OLSA01A
842 cizelu 73 void olsa_pulses(int count) // vytvori impulzy pro ridici logiku
829 cizelu 74 {
741 cizelu 75 int8 ct;
76 for(ct=0;ct<=count;ct++)
829 cizelu 77 {
730 cizelu 78 output_high(SCLK);
79 output_low(SCLK);
829 cizelu 80 }
81 }
730 cizelu 82  
842 cizelu 83 void olsa_pulse() // vytvori jeden impulz
829 cizelu 84 {
741 cizelu 85 output_high(SCLK);
86 output_low(SCLK);
829 cizelu 87 }
741 cizelu 88  
842 cizelu 89 void olsa_send(int8 info[8]) // USART komunikace s modulem OLSA01A - poslani zpravy
829 cizelu 90 {
842 cizelu 91 int *ip; // ukazatel na pole s informaci
92 int8 i; // pomocna promenna pro nastaveni 0 nebo 1 na SDIN
93 output_low(SDIN); // start bit
741 cizelu 94 olsa_pulse();
842 cizelu 95 for(ip=0;ip<8;ip++) // predani informace - 8 bit, LSB prvni > MSB posledni
829 cizelu 96 {
842 cizelu 97 i=info[ip]; // ziskani hodnoty z pole
98 if(i==1) // vyhodnoceni obsahu informace - nastav 1
829 cizelu 99 {
730 cizelu 100 output_high(SDIN);
829 cizelu 101 }
842 cizelu 102 else // vyhodnoceni obsahu informace - nastav 0
829 cizelu 103 {
730 cizelu 104 output_low(SDIN);
829 cizelu 105 }
741 cizelu 106 olsa_pulse();
829 cizelu 107 }
842 cizelu 108 output_high(SDIN); // stop bit
741 cizelu 109 olsa_pulse();
829 cizelu 110 }
730 cizelu 111  
842 cizelu 112 void olsa_reset() // hlavni RESET - provadi se po zapnuti
829 cizelu 113 {
741 cizelu 114 output_low(SDIN);
115 output_low(SCLK);
842 cizelu 116 olsa_pulses(30); // reset radkoveho senzoru
741 cizelu 117 output_high(SDIN);
842 cizelu 118 olsa_pulses(10); // start bit - synchronizace
741 cizelu 119 olsa_send(main_reset);
120 olsa_pulses(5);
121 olsa_send(set_mode_rg);
122 olsa_send(clear_mode_rg);
829 cizelu 123 }
789 cizelu 124  
842 cizelu 125 void olsa_setup() // kompletni nastaveni, provadi se po resetu
829 cizelu 126 {
842 cizelu 127 olsa_send(left_offset); // nastaveni leveho segmentu (offset a zisk)
789 cizelu 128 olsa_send(offset);
129 olsa_send(left_gain);
130 olsa_send(gain);
842 cizelu 131 olsa_send(mid_offset); // nastaveni prostredniho segmentu (offset a zisk)
789 cizelu 132 olsa_send(offset);
133 olsa_send(mid_gain);
134 olsa_send(gain);
842 cizelu 135 olsa_send(right_offset); // nastaveni praveho segmentu (offset a zisk)
789 cizelu 136 olsa_send(offset);
137 olsa_send(right_gain);
138 olsa_send(gain);
829 cizelu 139 }
790 cizelu 140  
842 cizelu 141 void olsa_integration() // snimani pixelu
829 cizelu 142 {
842 cizelu 143 olsa_send(start_int); // zacatek integrace senzoru
790 cizelu 144 olsa_pulses(22);
842 cizelu 145 olsa_send(stop_int); // konec integrace senzoru
790 cizelu 146 olsa_pulses(5);
829 cizelu 147 }
796 cizelu 148  
837 cizelu 149 void read_olsa()
150 {
842 cizelu 151 int8 cpixel; // pocet prectenych pixelu
152 int8 cbit; // pocet prectenych bitu
153 int8 pixel; // hodnota precteneho pixelu
837 cizelu 154 cpixel=0;
155 lp=0;
156 rp=0;
157 olsa_integration();
158 olsa_send(readout);
842 cizelu 159 do // precte 102 pixelu
837 cizelu 160 {
842 cizelu 161 if(!SDOUT) // zacatek prenosu - zachycen start bit
837 cizelu 162 {
163 pixel=0;
842 cizelu 164 for(cbit=0;cbit<8;cbit++) // cte jednotlive bity (8 bitu - 0 az 7)
837 cizelu 165 {
842 cizelu 166 olsa_pulse(); // impulz pro generovani dalsiho bitu
167 if(SDOUT) // zachycena 1
837 cizelu 168 {
842 cizelu 169 pixel|=1; // zapise do bitu pixelu 1 - OR
837 cizelu 170 }
842 cizelu 171 else // zachycena 0
837 cizelu 172 {
842 cizelu 173 pixel|=0; // zapise do bitu pixelu 0 - OR
837 cizelu 174 }
842 cizelu 175 pixel<<=1; // posune pixel
837 cizelu 176 }
842 cizelu 177 olsa_pulse(); // generuje stop bit
178 if(cpixel<52) // ulozeni do pole
837 cizelu 179 {
842 cizelu 180 olsa_lseg[lp]=pixel; // leva polovina radky - leve pole
837 cizelu 181 lp++;
182 }
183 else
184 {
842 cizelu 185 olsa_rseg[rp]=pixel; // prava polovina cary - prave pole
837 cizelu 186 rp++;
187 }
188 cpixel++;
189 }
190 else
191 {
842 cizelu 192 olsa_pulse(); // generuje start bit, nebyl-li poslan
837 cizelu 193 }
194 }
842 cizelu 195 while(cpixel<102); // precte 102 pixelu
837 cizelu 196 }
197  
741 cizelu 198 //ZACHRANNE SENZORY
842 cizelu 199 void read_blue_sensors() // cteni nouzovych senzoru
829 cizelu 200 {
842 cizelu 201 set_adc_channel(LINEL); // cti levy nouzovy senzor
700 cizelu 202 delay_us(10);
745 cizelu 203 line_l=read_adc();
842 cizelu 204 set_adc_channel(LINER); // cti pravy nouzovy senzor
700 cizelu 205 delay_us(10);
206 line_r=read_adc();
829 cizelu 207 }
730 cizelu 208  
700 cizelu 209 //PIPAK
716 cizelu 210 void beep(int16 period,int16 length)
829 cizelu 211 {
865 cizelu 212 int16 bp; // promenna pro nastaveni delky
213 for(bp=length;bp>0;bp--) // prepina vystupy tolikrat, jakou jsme zadali delku
829 cizelu 214 {
730 cizelu 215 output_high(SOUND_HI);
216 output_low(SOUND_LO);
217 delay_us(period);
218 output_high(SOUND_LO);
219 output_low(SOUND_HI);
220 delay_us(period);
829 cizelu 221 }
222 }
708 cizelu 223 //MOTORY
842 cizelu 224 void l_motor_fwd(int8 speedl) // levy motor dopredu
829 cizelu 225 {
708 cizelu 226 output_high(LMF);
227 output_low(LMB);
228 set_pwm2_duty(speedl);
829 cizelu 229 }
700 cizelu 230  
842 cizelu 231 void l_motor_bwd(int8 speedl) // levy motor dozadu
829 cizelu 232 {
708 cizelu 233 output_high(LMB);
234 output_low(LMF);
235 set_pwm2_duty(speedl);
829 cizelu 236 }
708 cizelu 237  
842 cizelu 238 void r_motor_fwd(int8 speedr) // pravy motor dopredu
829 cizelu 239 {
708 cizelu 240 output_high(RMF);
241 output_low(RMB);
242 set_pwm1_duty(speedr);
829 cizelu 243 }
708 cizelu 244  
842 cizelu 245 void r_motor_bwd(int8 speedr) // pravy motor dozadu
829 cizelu 246 {
708 cizelu 247 output_high(RMB);
248 output_low(RMF);
249 set_pwm1_duty(speedr);
829 cizelu 250 }
708 cizelu 251  
842 cizelu 252 void l_motor_off() // levy motor vypnut
829 cizelu 253 {
708 cizelu 254 output_low(LMF);
255 output_low(LMB);
256 set_pwm2_duty(0);
829 cizelu 257 }
730 cizelu 258  
842 cizelu 259 void r_motor_off() // pravy motor vypnut
829 cizelu 260 {
708 cizelu 261 output_low(RMF);
262 output_low(RMB);
263 set_pwm1_duty(0);
829 cizelu 264 }
708 cizelu 265  
842 cizelu 266 void motor_test() // test motoru
829 cizelu 267 {
708 cizelu 268 int8 i;
716 cizelu 269 beep(100,200);
799 cizelu 270 printf("TEST MOTORU\r\n");
708 cizelu 271 delay_ms(1000);
799 cizelu 272 printf("LEVY MOTOR DOPREDU\r\n");
818 cizelu 273 delay_ms(1000);
708 cizelu 274 for(i=0;i<255;i++)
829 cizelu 275 {
708 cizelu 276 l_motor_fwd(i);
799 cizelu 277 printf("RYCHLOST: %u\r\n",i);
716 cizelu 278 delay_ms(5);
829 cizelu 279 }
708 cizelu 280 for(i=255;i>0;i--)
829 cizelu 281 {
708 cizelu 282 l_motor_fwd(i);
799 cizelu 283 printf("RYCHLOST: %u\r\n",i);
716 cizelu 284 delay_ms(5);
829 cizelu 285 }
799 cizelu 286 printf("LEVY MOTOR DOZADU\r\n");
818 cizelu 287 delay_ms(1000);
708 cizelu 288 for(i=0;i<255;i++)
829 cizelu 289 {
708 cizelu 290 l_motor_bwd(i);
799 cizelu 291 printf("RYCHLOST: %u\r\n",i);
716 cizelu 292 delay_ms(5);
829 cizelu 293 }
708 cizelu 294 for(i=255;i>0;i--)
829 cizelu 295 {
708 cizelu 296 l_motor_bwd(i);
799 cizelu 297 printf("RYCHLOST: %u\r\n",i);
716 cizelu 298 delay_ms(5);
829 cizelu 299 }
799 cizelu 300 printf("PRAVY MOTOR DOPREDU\r\n");
818 cizelu 301 delay_ms(1000);
708 cizelu 302 for(i=0;i<255;i++)
829 cizelu 303 {
708 cizelu 304 r_motor_fwd(i);
799 cizelu 305 printf("RYCHLOST: %u\r\n",i);
716 cizelu 306 delay_ms(5);
829 cizelu 307 }
708 cizelu 308 for(i=255;i>0;i--)
829 cizelu 309 {
708 cizelu 310 r_motor_fwd(i);
799 cizelu 311 printf("RYCHLOST: %u\r\n",i);
716 cizelu 312 delay_ms(5);
829 cizelu 313 }
799 cizelu 314 printf("PRAVY MOTOR DOZADU\r\n");
818 cizelu 315 delay_ms(1000);
708 cizelu 316 for(i=0;i<255;i++)
829 cizelu 317 {
708 cizelu 318 r_motor_bwd(i);
799 cizelu 319 printf("RYCHLOST: %u\r\n",i);
716 cizelu 320 delay_ms(5);
829 cizelu 321 }
708 cizelu 322 for(i=255;i>0;i--)
829 cizelu 323 {
708 cizelu 324 r_motor_bwd(i);
799 cizelu 325 printf("RYCHLOST: %u\r\n",i);
716 cizelu 326 delay_ms(5);
829 cizelu 327 }
818 cizelu 328 l_motor_off();
329 r_motor_off();
799 cizelu 330 printf("KONEC TESTU MOTORU\r\n");
818 cizelu 331 delay_ms(1000);
829 cizelu 332 }
708 cizelu 333  
843 cizelu 334 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
708 cizelu 335 {
336 read_blue_sensors();
842 cizelu 337 printf("LEVA: %u \t",line_l);
338 delay_ms(10);
339 printf("PRAVA: %u \t",line_r);
340 delay_ms(10);
341 printf("L_NARAZ: %u \t",BUMPL);
342 delay_ms(10);
343 printf("P_NARAZ: %u \r\n",BUMPR);
344 delay_ms(10);
345 if(BUMPL&&BUMPR) // po zmacknuti stran narazniku spusti test motoru
708 cizelu 346 {
868 cizelu 347 beep(100,1000);
865 cizelu 348 printf("Levy naraznik - test OLSA\r\n");
349 printf("Pravy naraznik - test motoru\r\n");
350 delay_ms(500);
868 cizelu 351 while(true)
865 cizelu 352 {
868 cizelu 353 if(BUMPR)
843 cizelu 354 {
868 cizelu 355 beep(100,500); // pipni pri startu
356 motor_test();
357 }
358 if(BUMPL)
359 {
360 beep(100,500);
361 printf("TEST OLSA\r\n");
362 while(true)
843 cizelu 363 {
868 cizelu 364 int8 tisk;
365 int8 *tiskp;
366 read_olsa();
367 printf("cteni\r\n"); // po precteni vsech pixelu odradkuje
368 for(tiskp=0;tiskp<52;tiskp++) // tisk leve casti radky
369 {
370 tisk=olsa_lseg[tiskp];
371 printf("%x ",tisk);
372 }
373 for(tiskp=0;tiskp<52;tiskp++) // tisk prave casti radky
374 {
375 tisk=olsa_rseg[tiskp];
376 printf("%x ",tisk);
377 }
865 cizelu 378 }
379 }
380 }
381 }
708 cizelu 382 }
383  
700 cizelu 384 // HLAVNI SMYCKA
842 cizelu 385 void main()
829 cizelu 386 {
799 cizelu 387 printf("POWER ON \r\n");
842 cizelu 388 // NASTAVENI > provede se pouze pri zapnuti
865 cizelu 389 setup_adc_ports(sAN0-sAN1-sAN2); // aktivní analogové vstupy RA0, RA1 a RA2
789 cizelu 390 setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik
842 cizelu 391 setup_spi(SPI_SS_DISABLED);
392 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
393 setup_timer_1(T1_DISABLED);
394 setup_timer_2(T2_DIV_BY_16,255,1); // casovac pro PWM
789 cizelu 395 setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2
396 setup_ccp2(CCP_PWM); // povolí PWM na pinu RC1
842 cizelu 397 setup_comparator(NC_NC_NC_NC);
398 setup_vref(FALSE);
789 cizelu 399 l_motor_off(); // vypne levy motor
400 r_motor_off(); // vypne pravy motor
401 output_high(LED1); // zhasne LED1
402 output_high(LED2); // zhasne LED2
842 cizelu 403 olsa_reset();
404 olsa_setup();
868 cizelu 405 beep(100,500); // pipni pri startu
842 cizelu 406 printf("OK! \r\n");
407 delay_ms(500);
408 printf("VYBRAT MOD... \r\n");
409 while(true)
865 cizelu 410 {
868 cizelu 411 //diag();
837 cizelu 412 read_olsa();
865 cizelu 413 int8 searchp; // ukazatel na pole
414 int8 search; // ulozeni prectene hodnoty
415 int1 segment; // cara je vlevo nebo vpravo
416 int8 position; // ulozeni pozice cary
417 int8 protect_count; // opravdu vidime caru
418  
419 for(searchp=0;searchp<52;searchp++) // prohlizi levou cast cary
420 {
421 search=olsa_lseg[searchp]; // vybira pixel
868 cizelu 422 if(search==OLSA_LEV) // cerna nebo bila?
865 cizelu 423 {
424 protect_count++; // pokud nasleduje cerna, pricte 1 k poctu cernych pixelu
425 }
426 else
427 {
428 protect_count=0; // pokud nasleduje bila, pocet cernych pixelu vynuluje
429 }
430 if(protect_count>LINE_PX) // vidim caru
431 {
432 position=searchp; // zapis presnou pozici
433 segment=LEFT; // cara je v leve polovine
868 cizelu 434 searchp=55; // ukonci hledani
865 cizelu 435 }
436 }
868 cizelu 437 for(searchp=0;searchp<52;searchp++) // prohlizi pravou cast cary
865 cizelu 438 {
868 cizelu 439 search=olsa_rseg[searchp]; // vybira pixel
440 if(search==OLSA_LEV)
865 cizelu 441 {
442 protect_count++; // pokud nasleduje cerna, pricte 1 k poctu cernych pixelu
443 }
444 else
445 {
446 protect_count=0; // pokud nasleduje bila, pocet cernych pixelu vynuluje
447 }
448 if(protect_count>LINE_PX) // vidim caru
449 {
868 cizelu 450 position=(searchp+50); // zapis presnou pozici
865 cizelu 451 segment=RIGHT; // cara je v prave polovine
868 cizelu 452 searchp=55; // ukonci hledani
865 cizelu 453 }
454 }
455 printf("poloha: %u\r\n",position);
456 position=0;
730 cizelu 457 }
829 cizelu 458 }
865 cizelu 459