Rev 873 Rev 874
Line 1... Line 1...
1 #include "main.h" 1 #include "main.h"
2   2  
3 // NEPOUZIVAT PINY B6 A B7, JSOU VYHRAZENY PRO SERIOVOU KOMUNIKACI 3 // NEPOUZIVAT PINY B6 A B7, JSOU VYHRAZENY PRO SERIOVOU KOMUNIKACI
4 // BAUD RATE = 9600 4 // BAUD RATE = 9600
5 // ========================== PRIPRAVA DAT A VYSTUPU =========================== 5 // ========================== PRIPRAVA DAT A VYSTUPU ===========================
6 // pomocne promenne 6 // pomocne konstanty
7 #define LEFT 0 7 #define LEFT 0
8 #define RIGHT 1 8 #define RIGHT 1
-   9  
-   10 // konstanty a promenne pro sledovani cary
-   11 #define SPACE 10 // urcuje, za jak dlouho robot vi, ze nic nevidi
-   12 #define SPD1 100 // rychlost pomalejsiho motoru, kdyz je cara hodne z osy
-   13 #define SPD2 200 // rychlost pomalejsiho motoru, kdyz je cara z osy
-   14 #define SPD3 200 // rychlost rychlejsiho motoru pro vsechny pripady
-   15 #define SPD4 255 // pro oba motory - rovinka
-   16 // oblasti cary
-   17 #define L1 20 // nejvice vlevo
-   18 #define L2 40
-   19 #define L3 48
-   20 #define R1 52
-   21 #define R2 60
-   22 #define R3 80 // nejvice vpravo
-   23  
-   24 int8 action; // akce pro nastaveni motoru
-   25  
9 // univerzalni LED diody 26 // univerzalni LED diody
10 #define LED1 PIN_E0 27 #define LED1 PIN_E0
11 #define LED2 PIN_E1 28 #define LED2 PIN_E1
12   29  
13 // piezo pipak 30 // piezo pipak
Line 17... Line 34...
17 // radkovy senzor 34 // radkovy senzor
18 #define SDIN PIN_D4 // seriovy vstup 35 #define SDIN PIN_D4 // seriovy vstup
19 #define SDOUT input(PIN_C5) // seriovy vystup 36 #define SDOUT input(PIN_C5) // seriovy vystup
20 #define SCLK PIN_D5 // takt 37 #define SCLK PIN_D5 // takt
21   38  
22 #define LINE_PX 5 // pocet pixelu pro jiste urceni cary 39 #define LINE_PX 4 // pocet pixelu pro jiste urceni cary
23 #define OLSA_LEV 0x60 // rozhodovaci uroven (cca 10 odpovida cerne) 40 #define OLSA_LEV 0x60 // rozhodovaci uroven (cca 10 odpovida cerne)
24   41  
25 // pro komunikaci s OLSA, prvni se posila LSB 42 // pro komunikaci s OLSA, prvni se posila LSB
26 int main_reset[8]={1,1,0,1,1,0,0,0}; // hlavni reset 0x1B 43 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 44 int set_mode_rg[8]={1,1,1,1,1,0,1,0}; // zapis do MODE registru 0x5F
Line 47... Line 64...
47 int8 *rp; // ukazatel pro levou polovinu radky 64 int8 *rp; // ukazatel pro levou polovinu radky
48   65  
49 int8 position; // ulozeni pozice cary 66 int8 position; // ulozeni pozice cary
50 int8 old_position; // ulozeni predchozi pozice cary 67 int8 old_position; // ulozeni predchozi pozice cary
51 int1 line_sector; // cara je vlevo/vpravo 68 int1 line_sector; // cara je vlevo/vpravo
52 int8 change; // zmena oproti predchozi poloze -  
53 int8 gap; // pocita, jak dlouho neni videt cara 69 int8 gap; // pocita, jak dlouho neni videt cara
54   70  
55 // ================================= NARAZNIK ================================== 71 // ================================= NARAZNIK ==================================
56 #define BUMPL input(PIN_D6) 72 #define BUMPL input(PIN_D6)
57 #define BUMPR input(PIN_D7) 73 #define BUMPR input(PIN_D7)
Line 69... Line 85...
69 #define RMF PIN_D2 85 #define RMF PIN_D2
70 #define RMB PIN_D3 86 #define RMB PIN_D3
71   87  
72 int8 lm_speed; 88 int8 lm_speed;
73 int8 rm_speed; 89 int8 rm_speed;
-   90 int8 m;
74   91  
75 // =============================== PODPROGRAMY ================================= 92 // =============================== PODPROGRAMY =================================
76   93  
77 // ================================= OLSA01A =================================== 94 // ================================= OLSA01A ===================================
78   95  
Line 245... Line 262...
245 } 262 }
246 } 263 }
247   264  
248 // ============================ ZACHRANNE SENZORY ============================== 265 // ============================ ZACHRANNE SENZORY ==============================
249   266  
250 void read_blue_sensors() // cteni nouzovych senzoru 267 void read_blue_sensors() // cteni nouzovych senzoru
251 { 268 {
252 set_adc_channel(LINEL); // cti levy nouzovy senzor 269 set_adc_channel(LINEL); // cti levy nouzovy senzor
253 delay_us(10); 270 delay_us(10);
254 line_l=read_adc(); 271 line_l=read_adc();
255 set_adc_channel(LINER); // cti pravy nouzovy senzor 272 set_adc_channel(LINER); // cti pravy nouzovy senzor
256 delay_us(10); 273 delay_us(10);
257 line_r=read_adc(); 274 line_r=read_adc();
258 } 275 }
259 276
260 // ================================== PIPAK ==================================== 277 // ================================== PIPAK ====================================
261   278  
262 void beep(int16 period,int16 length) 279 void beep(int16 period,int16 length)
263 { 280 {
264 int16 bp; // promenna pro nastaveni delky 281 int16 bp; // promenna pro nastaveni delky
265 for(bp=length;bp>0;bp--) // prepina vystupy tolikrat, jakou jsme zadali delku 282 for(bp=length;bp>0;bp--) // prepina vystupy tolikrat, jakou jsme zadali delku
266 { 283 {
267 output_high(SOUND_HI); 284 output_high(SOUND_HI);
268 output_low(SOUND_LO); 285 output_low(SOUND_LO);
269 delay_us(period); 286 delay_us(period);
270 output_high(SOUND_LO); 287 output_high(SOUND_LO);
Line 273... Line 290...
273 } 290 }
274 } 291 }
275   292  
276 // ================================== MOTORY =================================== 293 // ================================== MOTORY ===================================
277   294  
278 void l_motor_fwd(int8 speedl) // levy motor dopredu 295 void l_motor_fwd(int8 speedl) // levy motor dopredu
279 { 296 {
280 output_high(LMF); 297 output_high(LMF);
281 output_low(LMB); 298 output_low(LMB);
282 set_pwm2_duty(speedl); 299 set_pwm2_duty(speedl);
283 } 300 }
284   301  
285 void l_motor_bwd(int8 speedl) // levy motor dozadu 302 void l_motor_bwd(int8 speedl) // levy motor dozadu
286 { 303 {
287 output_high(LMB); 304 output_high(LMB);
288 output_low(LMF); 305 output_low(LMF);
289 set_pwm2_duty(speedl); 306 set_pwm2_duty(speedl);
290 } 307 }
291   308  
292 void r_motor_fwd(int8 speedr) // pravy motor dopredu 309 void r_motor_fwd(int8 speedr) // pravy motor dopredu
293 { 310 {
294 output_high(RMF); 311 output_high(RMF);
295 output_low(RMB); 312 output_low(RMB);
296 set_pwm1_duty(speedr); 313 set_pwm1_duty(speedr);
297 } 314 }
298   315  
299 void r_motor_bwd(int8 speedr) // pravy motor dozadu 316 void r_motor_bwd(int8 speedr) // pravy motor dozadu
300 { 317 {
301 output_high(RMB); 318 output_high(RMB);
302 output_low(RMF); 319 output_low(RMF);
303 set_pwm1_duty(speedr); 320 set_pwm1_duty(speedr);
304 } 321 }
305   322  
306 void l_motor_off() // levy motor vypnut 323 void l_motor_off() // levy motor vypnut
307 { 324 {
308 output_low(LMF); 325 output_low(LMF);
309 output_low(LMB); 326 output_low(LMB);
310 set_pwm2_duty(0); 327 set_pwm2_duty(0);
311 } 328 }
312 329
313 void r_motor_off() // pravy motor vypnut 330 void r_motor_off() // pravy motor vypnut
314 { 331 {
315 output_low(RMF); 332 output_low(RMF);
316 output_low(RMB); 333 output_low(RMB);
317 set_pwm1_duty(0); 334 set_pwm1_duty(0);
318 } 335 }
319   336  
320 void motor_test() // test motoru 337 void motor_test() // TEST MOTORU
321 { 338 {
322 int8 i; 339 int8 i;
323 beep(100,200); 340 beep(100,200);
324 printf("TEST MOTORU\r\n"); 341 printf("TEST MOTORU\r\n");
325 delay_ms(1000); 342 delay_ms(1000);
326 printf("LEVY MOTOR DOPREDU\r\n"); 343 printf("LEVY MOTOR DOPREDU\r\n");
327 delay_ms(1000); 344 delay_ms(1000);
328 for(i=0;i<255;i++) // levy motor dopredu - zrychluje 345 for(i=0;i<255;i++) // levy motor dopredu - zrychluje
329 { 346 {
330 l_motor_fwd(i); 347 l_motor_fwd(i);
331 printf("RYCHLOST: %u\r\n",i); 348 printf("RYCHLOST: %u\r\n",i);
332 delay_ms(5); 349 delay_ms(5);
333 } 350 }
334 for(i=255;i>0;i--) // levy motor dopredu - zpomaluje 351 for(i=255;i>0;i--) // levy motor dopredu - zpomaluje
335 { 352 {
336 l_motor_fwd(i); 353 l_motor_fwd(i);
337 printf("RYCHLOST: %u\r\n",i); 354 printf("RYCHLOST: %u\r\n",i);
338 delay_ms(5); 355 delay_ms(5);
339 } 356 }
340 printf("LEVY MOTOR DOZADU\r\n"); // levy motor dozadu - zrychluje 357 printf("LEVY MOTOR DOZADU\r\n"); // levy motor dozadu - zrychluje
341 delay_ms(1000); 358 delay_ms(1000);
342 for(i=0;i<255;i++) 359 for(i=0;i<255;i++)
343 { 360 {
344 l_motor_bwd(i); 361 l_motor_bwd(i);
345 printf("RYCHLOST: %u\r\n",i); 362 printf("RYCHLOST: %u\r\n",i);
346 delay_ms(5); 363 delay_ms(5);
347 } 364 }
348 for(i=255;i>0;i--) // levy motor dozadu - zpomaluje 365 for(i=255;i>0;i--) // levy motor dozadu - zpomaluje
349 { 366 {
350 l_motor_bwd(i); 367 l_motor_bwd(i);
351 printf("RYCHLOST: %u\r\n",i); 368 printf("RYCHLOST: %u\r\n",i);
352 delay_ms(5); 369 delay_ms(5);
353 } 370 }
354 printf("PRAVY MOTOR DOPREDU\r\n"); 371 printf("PRAVY MOTOR DOPREDU\r\n");
355 delay_ms(1000); 372 delay_ms(1000);
356 for(i=0;i<255;i++) // pravy motor dopredu - zrychluje 373 for(i=0;i<255;i++) // pravy motor dopredu - zrychluje
357 { 374 {
358 r_motor_fwd(i); 375 r_motor_fwd(i);
359 printf("RYCHLOST: %u\r\n",i); 376 printf("RYCHLOST: %u\r\n",i);
360 delay_ms(5); 377 delay_ms(5);
361 } 378 }
362 for(i=255;i>0;i--) // pravy motor dopredu - zpomaluje 379 for(i=255;i>0;i--) // pravy motor dopredu - zpomaluje
363 { 380 {
364 r_motor_fwd(i); 381 r_motor_fwd(i);
365 printf("RYCHLOST: %u\r\n",i); 382 printf("RYCHLOST: %u\r\n",i);
366 delay_ms(5); 383 delay_ms(5);
367 } 384 }
368 printf("PRAVY MOTOR DOZADU\r\n"); 385 printf("PRAVY MOTOR DOZADU\r\n");
369 delay_ms(1000); 386 delay_ms(1000);
370 for(i=0;i<255;i++) // pravy motor dozadu - zrychluje 387 for(i=0;i<255;i++) // pravy motor dozadu - zrychluje
371 { 388 {
372 r_motor_bwd(i); 389 r_motor_bwd(i);
373 printf("RYCHLOST: %u\r\n",i); 390 printf("RYCHLOST: %u\r\n",i);
374 delay_ms(5); 391 delay_ms(5);
375 } 392 }
376 for(i=255;i>0;i--) // pravy motor dozadu - zpomaluje 393 for(i=255;i>0;i--) // pravy motor dozadu - zpomaluje
377 { 394 {
378 r_motor_bwd(i); 395 r_motor_bwd(i);
379 printf("RYCHLOST: %u\r\n",i); 396 printf("RYCHLOST: %u\r\n",i);
380 delay_ms(5); 397 delay_ms(5);
381 } 398 }
382 l_motor_off(); // po ukonceni testu vypnout motory 399 l_motor_off(); // po ukonceni testu vypnout motory
383 r_motor_off(); 400 r_motor_off();
384 printf("KONEC TESTU MOTORU\r\n"); 401 printf("KONEC TESTU MOTORU\r\n");
385 delay_ms(1000); 402 delay_ms(1000);
386 } 403 }
387   404  
388 // ================================ DIAGNOSTIKA ================================ 405 // ================================ DIAGNOSTIKA ================================
389   406  
390 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru 407 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
391 { 408 {
392 read_blue_sensors(); 409 read_blue_sensors();
393 read_olsa(); 410 read_olsa();
394 olsa_position(); 411 olsa_position();
395 printf("LEVA: %u \t",line_l); // tiskne z leveho senzoru 412 printf("LEVA: %u \t",line_l); // tiskne z leveho senzoru
396 printf("PRAVA: %u \t",line_r); // tiskne z praveho senzoru 413 printf("PRAVA: %u \t",line_r); // tiskne z praveho senzoru
397 printf("POLOHA: %u\t",position); // tiskne pozici OLSA 414 printf("POLOHA: %u\t",position); // tiskne pozici OLSA
398 printf("L_NARAZ: %u \t",BUMPL); // leve tlacitko narazniku 415 printf("L_NARAZ: %u \t",BUMPL); // leve tlacitko narazniku
399 printf("P_NARAZ: %u \r\n",BUMPR); // prave tlacitko narazniku 416 printf("P_NARAZ: %u \r\n",BUMPR); // prave tlacitko narazniku
400 if(BUMPL&&BUMPR) // po zmacknuti stran narazniku spusti test motoru 417 if(BUMPL&&BUMPR) // po zmacknuti stran narazniku spusti test motoru
401 { 418 {
402 beep(100,1000); 419 beep(100,1000);
403 printf("Levy naraznik - test OLSA\r\n"); 420 printf("Levy naraznik - test OLSA\r\n");
404 printf("Pravy naraznik - test motoru\r\n"); 421 printf("Pravy naraznik - test motoru\r\n");
405 delay_ms(500); 422 delay_ms(500);
Line 463... Line 480...
463 delay_ms(500); 480 delay_ms(500);
464 printf("VYBRAT MOD... \r\n"); 481 printf("VYBRAT MOD... \r\n");
465 while(true) 482 while(true)
466 { 483 {
467 // ============================ HLAVNI CAST PROGRAMU =========================== 484 // ============================ HLAVNI CAST PROGRAMU ===========================
468 if(true) // spusteni hledani pravym naraznikem 485 if(BUMPR) // spusteni hledani pravym naraznikem
469 { 486 {
470 beep(100,500); 487 beep(100,500);
471 while(true) 488 while(true)
472 { 489 {
473 // =============================== SLEDOVANI CARY ============================== 490 // =============================== SLEDOVANI CARY ==============================
Line 475... Line 492...
475 read_olsa(); // precte a ulozi hodnoty z olsa 492 read_olsa(); // precte a ulozi hodnoty z olsa
476 olsa_position(); // vyhodnoti pozici cary 493 olsa_position(); // vyhodnoti pozici cary
477 read_blue_sensors(); // cte nouzove senzory 494 read_blue_sensors(); // cte nouzove senzory
478 if(line_l==0) // robot najede na levy nouzovy senzor 495 if(line_l==0) // robot najede na levy nouzovy senzor
479 { 496 {
480 l_motor_bwd(255); // prudce zatoc doleva 497 l_motor_bwd(150); // prudce zatoc doleva
481 r_motor_fwd(255); 498 r_motor_fwd(255);
482 line_sector=LEFT; // pamatuj, kde je cara 499 line_sector=LEFT; // pamatuj, kde je cara
483 delay_ms(20); 500 delay_ms(200);
484 } 501 }
485 if(line_r==0) // robot najede na pravy nouzovy senzor 502 if(line_r==0) // robot najede na pravy nouzovy senzor
486 { 503 {
487 l_motor_fwd(255); // prudce zatoc doprava 504 l_motor_fwd(255); // prudce zatoc doprava
488 r_motor_bwd(255); 505 r_motor_bwd(150);
489 line_sector=RIGHT; // pamatuj, kde je cara 506 line_sector=RIGHT; // pamatuj, kde je cara
490 delay_ms(20); 507 delay_ms(200);
491 } -  
492 if(position>old_position) // pocita absolutni rozdil mezi soucasnou a predchozi pozici -  
493 { -  
494 change=(position-old_position); // pozice ma vyssi cislo nez predchozi pozice -  
495 } -  
496 else -  
497 { -  
498 change=(old_position-position); // pozice ma vyssi cislo nez predchozi pozice -  
499 } 508 }
500 if(position==0) // pokud neni videt cara 509 if(position==0) // pokud neni videt cara
501 { 510 {
502 position=old_position; // nastav predchozi pozici 511 position=old_position; // nastav predchozi pozici
503 gap++; // pocita, jak dlouho neni videt cara 512 gap++; // pocita, jak dlouho neni videt cara
504 } 513 }
505 else // pokud je videt cara 514 else // pokud je videt cara
506 { 515 {
507 gap=0; // vynuluje citani 516 gap=0; // vynuluje citani
-   517 m=0; // pry svetsovani hulu zataceni
-   518 }
-   519 if(gap>SPACE) // dlouho neni nic videt
-   520 {
-   521 if(line_sector==LEFT) // cara byla naposledy vlevo
-   522 {
-   523 l_motor_bwd(50+m); // zatacej doleva
-   524 r_motor_fwd(200);
-   525 m++;
-   526 }
-   527 else // cara byla naposledy vpravo
-   528 {
-   529 r_motor_bwd(50+m); // zatacej doprava
-   530 l_motor_fwd(200);
-   531 m++;
-   532 }
-   533 }
-   534 if(position>0) // urceni akci pro rizeni, pokud cara je videt
-   535 {
-   536 if(position>L1) // cara je hodne vlevo
-   537 {
-   538 if(position>L2) // cara je vlevo
-   539 {
-   540 if(position>L3) // cara je mirne vlevo
-   541 {
-   542 if(position>R1) // cara je mirne vpravo
-   543 {
-   544 if(position>R2) // cara je vpravo
-   545 {
-   546 if(position>R3) // cara je hodne vpravo
-   547 {
-   548 action=6;
-   549 }
-   550 }
-   551 else
-   552 {
-   553 action=5;
-   554 }
-   555 }
-   556 else
-   557 {
-   558 action=4;
-   559 }
-   560 }
-   561 else
-   562 {
-   563 action=3;
-   564 }
-   565 }
-   566 else
-   567 {
-   568 action=2;
-   569 }
-   570 }
-   571 else
-   572 {
-   573 action=1;
-   574 }
-   575 }
-   576 if(L3<position>R1) // cara je uprostred
-   577 {
-   578 action=7;
-   579 }
-   580 switch(action) // reakce na pohyb cary
-   581 {
-   582 case 1: // cara je hodne vlevo
-   583 rm_speed=(SPD3+position);
-   584 lm_speed=(SPD1+position);
-   585 r_motor_fwd(rm_speed);
-   586 l_motor_bwd(lm_speed);
-   587 break;
-   588 case 2: // cara je vlevo
-   589 rm_speed=(SPD3+position);
-   590 r_motor_fwd(rm_speed);
-   591 l_motor_off();
-   592 break;
-   593 case 3: // cara je mirne vlevo
-   594 rm_speed=(SPD3+position);
-   595 lm_speed=(SPD2-position);
-   596 r_motor_fwd(rm_speed);
-   597 l_motor_fwd(lm_speed);
-   598 break;
-   599 case 4: // cara je mirne vpravo
-   600 lm_speed=(SPD3+(position/2));
-   601 rm_speed=(SPD2-(position/2));
-   602 l_motor_fwd(lm_speed);
-   603 r_motor_fwd(rm_speed);
-   604 break;
-   605 case 5: // cara je vpravo
-   606 lm_speed=(SPD3+(position/2));
-   607 l_motor_fwd(lm_speed);
-   608 r_motor_off();
-   609 break;
-   610 case 6: // cara je hodne vpravo
-   611 lm_speed=(SPD3+(position/2));
-   612 rm_speed=(SPD2-(position/2));
-   613 l_motor_fwd(lm_speed);
-   614 r_motor_bwd(rm_speed);
-   615 break;
-   616 case 7: // cara je uprostred
-   617 l_motor_fwd(SPD4);
-   618 r_motor_fwd(SPD4);
-   619 break;
508 } 620 }
509 } 621 }
510 } 622 }
511 } 623 }
512 } 624 }