Otto - riadiaci program v.2
Revision as of 08:45, 3 August 2018 by 158.195.189.186 (talk)
Program umožňuje priame riadenie robota (aj cez BlueTooth). Teraz už funguje aj na pinoch 2,4 a môže sa naraz programovať aj komunikovať.
Na komunikáciu s robotom môžete použiť napr. program Putty: putty.exe.
| klaves | servo | pin |
|---|---|---|
| a/q | ľavá ruka | 10 |
| ;/p | pravá ruka | 11 |
| z/x | ľavá noha | 9 |
| ,/. | pravá noha | 6 |
| d/c | ľavá päta | 5 |
| k/m | pravá päta | 3 |
| 1/9 | zníž/zvýš rýchlosť pohybu | - |
| TXD | BT | 2 |
| RXD | BT | 4 |
| TRIG | UZ | 7 |
| ECHO | UZ | 8 |
| Sirena | 12 |
Ďalej umožňuje
- kalibrovať strednú polohu servomotorov (pomocou klávesu H)
- pomocou + a - najdeme nulovu polohu serva
- eneter ideme na dalsie servo
- ukoncime E ulozenie do eprom
- potvrdime Y
- kalibrovať limity pre všetky stupne voľnosti (pomocou klávesu J), zobrazenie celej kalibrácie: kláves G
- celú kalibráciu je možné zapísať do trvalej pamäte EEPROM pomocou klávesu E (používať opatrne!), po zapnutí sa automaticky načíta
- pozdraví po klávese -, ruky dá hore na kláves R
- načíta choreografiu po klávese @,
- zobrazí načítanú choreografiu na kláves ? a
- zatancuje načítanú choreografiu - kláves t.
- Na klávesách 3-6 je možné nastaviť rôznu kombináciu (funkcie kombinacia1() - kombinacia4()).
- Kláves U testuje ultrazvuk
- Medzera resetne všetky servo motory do strednej polohy.
1 #include <Servo.h>
2 #include <EEPROM.h>
3
4 #define ECHO_BT_TO_USB 1
5
6 #define US_TRIG 7
7 #define US_ECHO 8
8
9 #define BT_RX 2
10 #define BT_TX 4
11
12 #define SIRENA 12
13
14 //maximalna dlzka choreografie
15 #define CHOREO_LEN 200
16
17 // cisla pinov, kde su zapojene servo motory
18 #define PIN_SERVO_LAVA_RUKA 10
19 #define PIN_SERVO_PRAVA_RUKA 11
20 #define PIN_SERVO_LAVA_NOHA 9
21 #define PIN_SERVO_PRAVA_NOHA 6
22 #define PIN_SERVO_LAVA_PATA 5
23 #define PIN_SERVO_PRAVA_PATA 3
24
25 #define S1 3
26 #define S2 5
27 #define S3 6
28 #define S4 9
29 #define S5 10
30 #define S6 11
31 #define SIRENA 12
32
33 //maximalna dlzka choreografie
34 #define CHOREO_LEN 200
35
36 // tu su serva cislovane 1-6
37 #define SERVO_LAVA_RUKA 5
38 #define SERVO_PRAVA_RUKA 6
39 #define SERVO_LAVA_NOHA 4
40 #define SERVO_PRAVA_NOHA 3
41 #define SERVO_LAVA_PATA 2
42 #define SERVO_PRAVA_PATA 1
43
44 // ak su niektore serva naopak, je tu jednotka
45 uint8_t servo_invertovane[6] = {0, 0, 1, 1, 0, 1};
46
47 // znaky, ktorymi sa ovladaju jednotlive stupne volnosti
48 char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' };
49 // co robia jednotlive znaky (znamienko urcuje smer)
50 int8_t zmeny[] = {SERVO_LAVA_RUKA, -SERVO_LAVA_RUKA,
51 SERVO_PRAVA_RUKA, -SERVO_PRAVA_RUKA,
52 -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA,
53 -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA,
54 SERVO_LAVA_PATA, -SERVO_LAVA_PATA,
55 SERVO_PRAVA_PATA, -SERVO_PRAVA_PATA };
56
57 // sem si mozno ulozit svoju kalibraciu
58 //uint8_t prednastavena_kalibracia[] = { 78, 69, 83, 80, 50, 67 };
59 uint8_t prednastavena_kalibracia[] = { 90, 90, 90, 90, 90, 90 };
60
61 uint8_t dolny_limit[] = {0, 0, 0, 0, 0, 0, 0, 0};
62 uint8_t horny_limit[] = {180, 180, 180, 180, 180, 180};
63
64 Servo s[6];
65
66 uint16_t ch_time[CHOREO_LEN];
67 uint8_t ch_servo[CHOREO_LEN];
68 uint8_t ch_val[CHOREO_LEN];
69 int ch_len;
70 uint8_t kalib[6];
71 int stav[6];
72 int krok;
73
74 void setup() {
75 Serial.begin(9600);
76 init_serial(9600);
77 init_ultrasonic();
78
79 randomSeed(analogRead(1));
80 s[0].attach(PIN_SERVO_PRAVA_PATA);
81 s[1].attach(PIN_SERVO_LAVA_PATA);
82 s[2].attach(PIN_SERVO_PRAVA_NOHA);
83 s[3].attach(PIN_SERVO_LAVA_NOHA);
84 s[4].attach(PIN_SERVO_PRAVA_RUKA);
85 s[5].attach(PIN_SERVO_LAVA_RUKA);
86 precitaj_kalibraciu_z_EEPROM();
87 for (int i = 0; i < 6; i++)
88 {
89 kalib[i] = prednastavena_kalibracia[i];
90 stav[i] = kalib[i];
91 s[i].write(stav[i]);
92 }
93 ch_len = 0;
94 krok = 7;
95 ahoj();
96 ruky2();
97 delay(100);
98 serial_println("\r\n Otto DTDT");
99 }
100
101 void loop() {
102 char z = -1;
103 if (serial_available()) z = serial_read();
104 #ifdef ECHO_BT_TO_USB
105 if (Serial.available()) z = Serial.read();
106 #endif
107
108 if (z != -1)
109 {
110 if (pohyb_znakom(z)) return;
111 else if (pohyb_kombinacia(z)) return;
112 else if (z == '@') nacitaj_choreografiu();
113 else if (z == '?') vypis_choreografiu();
114 else if (z == 't') zatancuj_choreografiu();
115 else if (z == '-') ahoj();
116 else if (z == ' ') reset();
117 else if (z == 'H') kalibruj();
118 else if (z == 'J') nastav_limity();
119 else if (z == 'G') vypis_kalibraciu();
120 else if (z == 'L') nacitaj_kalibraciu();
121 else if (z == 'E') zapis_kalibraciu_do_EEPROM();
122 else if (z == 'R') ruky();
123 else if (z == '9') zvys_krok();
124 else if (z == '1') zniz_krok();
125 else if (z == 'U') test_ultrazvuk();
126 }
127 int16_t d = measure_distance();
128 if (d < 10) menu_ultrasonic_request();
129 }
130
131 void test_ultrazvuk()
132 {
133 int i = 0;
134 while ((serial_available() == 0) && (Serial.available() == 0))
135 {
136 serial_println_num(measure_distance());
137 delay(100);
138 }
139 serial_read();
140 }
141
142 void menu_ultrasonic_request()
143 {
144 uint32_t tm = millis();
145 int d = measure_distance();
146 while ((millis() - tm < 3000) && (d < 15))
147 {
148 delay(10);
149 d = measure_distance();
150 }
151 if (d < 15)
152 ultrasonic_menu();
153 }
154
155 void ultrasonic_menu()
156 {
157 int selection = 0;
158 tone(SIRENA, 880, 200);
159
160 do {
161 int count = 0;
162 do {
163 int32_t d = measure_distance();
164 if (d == 10000) continue;
165 if (d >= 20) count++;
166 else count = 0;
167 delay(10);
168 } while (!serial_available() && !Serial.available() && (count < 20));
169
170 tone(SIRENA, 440, 200);
171 uint32_t tm = millis();
172 while ((measure_distance() > 15) && (millis() - tm < 1500) && !serial_available() && !Serial.available()) delay(10);
173 if (millis() - tm >= 1500)
174 {
175 tone(SIRENA, 2000, 50);
176 menu_command(selection);
177 return;
178 }
179 selection++;
180 for (int i = 0; i < selection; i++)
181 {
182 tone(SIRENA, 1261, 50);
183 delay(250);
184 }
185 } while (!serial_available() && !Serial.available());
186 while (serial_available()) serial_read();
187 while (Serial.available()) Serial.read();
188 }
189
190 void menu_command(int cmd)
191 {
192 serial_println_num(cmd);
193 }
194
195 void precitaj_kalibraciu_z_EEPROM()
196 {
197 uint8_t value = EEPROM.read(1);
198 if (value != '~') return;
199 for (int i = 2; i < 8; i++)
200 prednastavena_kalibracia[i - 2] = EEPROM.read(i);
201 for (int i = 0; i < 6; i++)
202 dolny_limit[i] = EEPROM.read(i + 9);
203 for (int i = 0; i < 6; i++)
204 horny_limit[i] = EEPROM.read(i + 15);
205 }
206
207 char read_char()
208 {
209 while (!serial_available() && !Serial.available());
210 if (serial_available()) return serial_read();
211 else return Serial.read();
212 }
213
214 void zapis_kalibraciu_do_EEPROM()
215 {
216 serial_print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
217 char odpoved = read_char();
218 serial_println_char(odpoved);
219 if (odpoved == 'Y')
220 {
221 EEPROM.write(1, '~');
222 for (int i = 2; i < 8; i++)
223 EEPROM.write(i, kalib[i - 2]);
224 for (int i = 0; i < 6; i++)
225 EEPROM.write(9 + i, dolny_limit[i]);
226 for (int i = 0; i < 6; i++)
227 EEPROM.write(15 + i, horny_limit[i]);
228 serial_println("ok");
229 }
230 }
231
232 void pipni()
233 {
234 tone(SIRENA, 1568, 50);
235 delay(100);
236 tone(SIRENA, 1357, 50);
237 }
238
239 void ruky()
240 {
241 int odloz_krok = krok;
242 delay(500);
243 krok = 90;
244 pohyb(SERVO_LAVA_RUKA);
245 pohyb(SERVO_PRAVA_RUKA);
246 delay(1000);
247 krok = 180;
248 pohyb(-SERVO_LAVA_RUKA);
249 pohyb(-SERVO_PRAVA_RUKA);
250 delay(1000);
251 krok = odloz_krok;
252 pipni();
253 }
254
255 void ruky2()
256 {
257 int odloz_krok = krok;
258 delay(500);
259 krok = 180;
260 pohyb(SERVO_LAVA_RUKA);
261 pohyb(SERVO_PRAVA_RUKA);
262 delay(1000);
263 krok = 90;
264 pohyb(-SERVO_LAVA_RUKA);
265 pohyb(-SERVO_PRAVA_RUKA);
266 delay(1000);
267 krok = odloz_krok;
268 pipni();
269 }
270
271 void ahoj()
272 {
273 tone(SIRENA, 1568, 50);
274 delay(70);
275 tone(SIRENA, 1175, 30);
276 delay(50);
277 tone(SIRENA, 880, 30);
278 delay(50);
279 tone(SIRENA, 1047, 50);
280 delay(70);
281 tone(SIRENA, 1245, 30);
282 delay(150);
283 tone(SIRENA, 1568, 50);
284 delay(100);
285 if (random(10) > 4) tone(SIRENA, 1357, 50);
286 else tone(SIRENA, 1047, 50);
287 }
288
289 void nastav_koncatinu(int8_t servo, uint8_t poloha)
290 {
291 int8_t srv = (servo > 0)?servo:-servo;
292 srv--;
293 poloha += kalib[srv] - 90;
294 if (poloha > 180) poloha = 180;
295 if (poloha < 0) poloha = 0;
296 stav[srv] = poloha;
297 s[srv].write(stav[srv]);
298 }
299
300 void pohyb(int8_t servo)
301 {
302 int8_t srv = (servo > 0)?servo:-servo;
303 srv--;
304 if (servo_invertovane[srv]) servo = -servo;
305 if (servo > 0)
306 {
307 if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
308 else stav[srv] = horny_limit[srv];
309 s[srv].write(stav[srv]);
310 }
311 else if (servo < 0)
312 {
313 if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok;
314 else stav[srv] = dolny_limit[srv];
315 s[srv].write(stav[srv]);
316 }
317 }
318
319 uint8_t pohyb_znakom(char z)
320 {
321 for (int i = 0; i < 12; i++)
322 {
323 if (z == znaky_zmien[i])
324 {
325 int8_t servo = zmeny[i];
326 pohyb(servo);
327 }
328 }
329 }
330
331 void kombinacia1()
332 {
333 pohyb(SERVO_LAVA_NOHA);
334 pohyb(-SERVO_PRAVA_PATA);
335 }
336
337 void kombinacia2()
338 {
339 pohyb(SERVO_PRAVA_NOHA);
340 pohyb(-SERVO_LAVA_PATA);
341 }
342
343 void kombinacia3()
344 {
345 pohyb(SERVO_LAVA_RUKA);
346 pohyb(SERVO_PRAVA_RUKA);
347 }
348
349 void kombinacia4()
350 {
351 pohyb(-SERVO_LAVA_RUKA);
352 pohyb(-SERVO_PRAVA_RUKA);
353 }
354
355 int pohyb_kombinacia(char z)
356 {
357 if (z == '3') kombinacia1();
358 else if (z == '4') kombinacia2();
359 else if (z == '5') kombinacia3();
360 else if (z == '6') kombinacia4();
361 else return 0;
362 return 1;
363 }
364
365 int nacitajCislo()
366 {
367 int num = 0;
368 int z;
369 do {
370 z = serial_read();
371 if (z == '#') while (z != 13) z = serial_read();
372 } while ((z < '0') || (z > '9'));
373 while ((z >= '0') && (z <= '9'))
374 {
375 num *= 10;
376 num += (z - '0');
377 do { z = serial_read(); if (z == -1) delayMicroseconds(10); } while (z < 0);
378 }
379 return num;
380 }
381
382 void nacitaj_choreografiu()
383 {
384 ch_len = 0;
385 int tm;
386 do {
387 tm = nacitajCislo();
388 ch_time[ch_len] = tm;
389 ch_servo[ch_len] = nacitajCislo();
390 ch_val[ch_len] = nacitajCislo();
391 ch_len++;
392 if (ch_len == CHOREO_LEN) break;
393 } while (tm > 0);
394 pipni();
395 }
396
397 void vypis_choreografiu()
398 {
399 for (int i = 0; i < ch_len; i++)
400 {
401 serial_print_num(ch_time[i]);
402 serial_print(" ");
403 serial_print_num(ch_servo[i]);
404 serial_print(" ");
405 serial_println_num(ch_val[i]);
406 }
407 pipni();
408 }
409
410 void zatancuj_choreografiu()
411 {
412 for (int i = 0; i < ch_len; i++)
413 {
414 delay(ch_time[i]);
415 nastav_koncatinu(ch_servo[i], ch_val[i]);
416 }
417 if (ch_len > 0) stav[ch_servo[ch_len - 1]] = ch_val[ch_len - 1];
418 pipni();
419 }
420
421 void reset()
422 {
423 for (int i = 0; i < 6; i++)
424 {
425 stav[i] = kalib[i];
426 s[i].write(kalib[i]);
427 }
428 pipni();
429 }
430
431 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
432 {
433 serial_print(" (+/-/ENTER): ");
434 serial_println_num(hodnota);
435 s[servo].write(hodnota);
436 char z;
437 do {
438 z = read_char();
439 if ((z == '+') && (hodnota < 180)) hodnota++;
440 else if ((z == '-') && (hodnota > 0)) hodnota--;
441 if ((z == '+') || (z == '-'))
442 {
443 serial_print_num(hodnota); serial_print_char('\r');
444 s[servo].write(hodnota);
445 }
446 } while (z != 13);
447 return hodnota;
448 }
449
450 void kalibruj()
451 {
452 for (int i = 0; i < 6; i++)
453 {
454 serial_print_num(i);
455 kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
456 serial_print_num(i);
457 serial_print(": ");
458 serial_println_num(kalib[i]);
459 }
460 for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
461 serial_println("ok");
462 pipni();
463 }
464
465 void nastav_limity()
466 {
467 for (int i = 0; i < 6; i++)
468 {
469 serial_print_num(i);
470 serial_print("dolny");
471 dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
472 serial_print_num(i);
473 serial_print(" dolny: ");
474 serial_println_num(dolny_limit[i]);
475 s[i].write(kalib[i]);
476
477 serial_print_num(i);
478 serial_print("horny");
479 horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
480 serial_print_num(i);
481 serial_print(" horny: ");
482 serial_println_num(horny_limit[i]);
483 s[i].write(kalib[i]);
484 }
485 for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print("-"); serial_print_num(horny_limit[i]); serial_print(" "); }
486 serial_println("ok");
487 pipni();
488 }
489
490 void vypis_kalibraciu()
491 {
492 serial_print("stredy: ");
493 for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
494 serial_println();
495 serial_print("dolny limit: ");
496 for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print(" "); }
497 serial_println();
498 serial_print("horny limit: ");
499 for (int i = 0; i < 6; i++) { serial_print_num(horny_limit[i]); serial_print(" "); }
500 serial_println();
501 }
502
503 void nacitaj_kalibraciu()
504 {
505 int tm;
506 for (int i = 0; i < 6; i++)
507 kalib[i] = nacitajCislo();
508 vypis_kalibraciu();
509 serial_println("ok");
510 pipni();
511 }
512
513 void zvys_krok()
514 {
515 if (krok < 180) krok++;
516 serial_print("krok: ");
517 serial_println_num(krok);
518 }
519
520 void zniz_krok()
521 {
522 if (krok > 0) krok--;
523 serial_print("krok: ");
524 serial_println_num(krok);
525 }
526
527 // nasleduje softverova implementacia serioveho portu
528 #define SERIAL_STATE_IDLE 0
529 #define SERIAL_STATE_RECEIVING 1
530 #define SERIAL_BUFFER_LENGTH 20
531
532 static volatile uint8_t serial_state;
533 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
534 static volatile uint8_t serial_buf_wp, serial_buf_rp;
535
536 static volatile uint8_t receiving_byte;
537
538 static volatile uint32_t time_startbit_noticed;
539 static volatile uint8_t next_bit_order;
540 static volatile uint8_t waiting_stop_bit;
541 static uint16_t one_byte_duration;
542 static uint16_t one_bit_duration;
543 static uint16_t one_bit_write_duration;
544 static uint16_t half_of_one_bit_duration;
545
546 void init_serial(uint32_t baud_rate)
547 {
548 pinMode(2, INPUT);
549 pinMode(4, OUTPUT);
550
551 serial_state = SERIAL_STATE_IDLE;
552
553 one_byte_duration = 9500000 / baud_rate;
554 one_bit_duration = 1000000 / baud_rate;
555 one_bit_write_duration = one_bit_duration - 1;
556 half_of_one_bit_duration = 500000 / baud_rate;
557
558 PCMSK2 |= 4; //PCINT18;
559 PCIFR &= ~4; //PCIF2;
560 PCICR |= 4; // PCIE2;
561 }
562
563 ISR(PCINT2_vect)
564 {
565 uint32_t tm = micros();
566 if (serial_state == SERIAL_STATE_IDLE)
567 {
568 time_startbit_noticed = tm;
569 serial_state = SERIAL_STATE_RECEIVING;
570 receiving_byte = 0xFF;
571 next_bit_order = 0;
572 }
573 else if (tm - time_startbit_noticed > one_byte_duration)
574 {
575 serial_buffer[serial_buf_wp] = receiving_byte;
576 serial_buf_wp++;
577 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
578 time_startbit_noticed = tm;
579 receiving_byte = 0xFF;
580 next_bit_order = 0;
581 }
582 else if (PIND & 4)
583 {
584 int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
585 while (next_bit_order < new_next_bit_order)
586 {
587 receiving_byte &= ~(1 << next_bit_order);
588 next_bit_order++;
589 }
590 if (next_bit_order == 8)
591 {
592 serial_buffer[serial_buf_wp] = receiving_byte;
593 serial_buf_wp++;
594 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
595 serial_state = SERIAL_STATE_IDLE;
596 }
597 } else
598 next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
599 }
600
601 uint8_t serial_available()
602 {
603 cli();
604 if (serial_buf_rp != serial_buf_wp)
605 {
606 sei();
607 return 1;
608 }
609 if (serial_state == SERIAL_STATE_RECEIVING)
610 {
611 uint32_t tm = micros();
612 if (tm - time_startbit_noticed > one_byte_duration)
613 {
614 serial_state = SERIAL_STATE_IDLE;
615 serial_buffer[serial_buf_wp] = receiving_byte;
616 serial_buf_wp++;
617 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
618 sei();
619 return 1;
620 }
621 }
622 sei();
623 return 0;
624 }
625
626 int16_t serial_read()
627 {
628 cli();
629 if (serial_buf_rp != serial_buf_wp)
630 {
631 uint8_t ch = serial_buffer[serial_buf_rp];
632 serial_buf_rp++;
633 if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
634 sei();
635 return ch;
636 }
637
638 if (serial_state == SERIAL_STATE_RECEIVING)
639 {
640 uint32_t tm = micros();
641 if (tm - time_startbit_noticed > one_byte_duration)
642 {
643 uint8_t ch = receiving_byte;
644 serial_state = SERIAL_STATE_IDLE;
645 sei();
646 return ch;
647 }
648 }
649 sei();
650 return -1;
651 }
652
653 void serial_write(uint8_t ch)
654 {
655 #ifdef ECHO_BT_TO_USB
656 Serial.print((char)ch);
657 #endif
658 PORTD &= ~16;
659 delayMicroseconds(one_bit_write_duration);
660 for (uint8_t i = 0; i < 8; i++)
661 {
662 if (ch & 1) PORTD |= 16;
663 else PORTD &= ~16;
664 ch >>= 1;
665 delayMicroseconds(one_bit_write_duration);
666 }
667 PORTD |= 16;
668 delayMicroseconds(one_bit_write_duration);
669 delayMicroseconds(one_bit_write_duration);
670 delayMicroseconds(one_bit_write_duration);
671 delayMicroseconds(one_bit_write_duration);
672 delayMicroseconds(one_bit_write_duration);
673 }
674
675 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
676 {
677 uint16_t len;
678 uint16_t ch;
679 do {
680 ch = serial_read();
681 if (ch == 13) continue;
682 } while (ch == -1);
683
684 do {
685 if ((ch != 13) && (ch != 10) && (ch != -1))
686 {
687 *(ln++) = ch;
688 max_length--;
689 len++;
690 }
691 ch = serial_read();
692 } while ((ch != 13) && max_length);
693 *ln = 0;
694 return len;
695 }
696
697 void serial_print_num(int32_t number)
698 {
699 if (number < 0)
700 {
701 serial_write('-');
702 number = -number;
703 }
704 int32_t rad = 1;
705 while (number / rad) rad *= 10;
706 if (number > 0) rad /= 10;
707 while (rad)
708 {
709 serial_write((char)('0' + (number / rad)));
710 number -= (number / rad) * rad;
711 rad /= 10;
712 }
713 }
714
715 void serial_print_char(char ch)
716 {
717 serial_write(ch);
718 }
719
720 void serial_print(const uint8_t *str)
721 {
722 while (*str) serial_write(*(str++));
723 }
724
725 void serial_println(const uint8_t *str)
726 {
727 serial_print(str);
728 serial_write(13);
729 serial_write(10);
730 }
731
732 void serial_println_num(int32_t number)
733 {
734 serial_print_num(number);
735 serial_println();
736 }
737
738 void serial_println_char(char ch)
739 {
740 serial_write(ch);
741 serial_println();
742 }
743
744 void serial_println()
745 {
746 serial_write(13);
747 serial_write(10);
748 }
749
750 // nasleduje citanie z utltazvukoveho senzora
751
752 static volatile uint32_t pulse_start;
753 static volatile int16_t distance;
754 static volatile uint8_t new_distance;
755
756 void init_ultrasonic()
757 {
758 pinMode(US_ECHO, INPUT);
759 pinMode(US_TRIG, OUTPUT);
760
761 PCMSK0 |= 1; //PCINT0;
762 PCIFR &= ~1; //PCIF0;
763 PCICR |= 1; // PCIE0;
764 }
765
766 ISR(PCINT0_vect)
767 {
768 if (PINB & 1) pulse_start = micros();
769 else
770 {
771 distance = (int16_t)((micros() - pulse_start) / 58);
772 new_distance = 1;
773 }
774 }
775
776 void start_distance_measurement()
777 {
778 distance = 10000;
779 new_distance = 0;
780 digitalWrite(US_TRIG, HIGH);
781 delayMicroseconds(10);
782 digitalWrite(US_TRIG, LOW);
783 }
784
785 void wait_for_distance_measurement_to_complete()
786 {
787 uint8_t counter = 0;
788 while ((counter < 20) && !new_distance)
789 {
790 delay(1);
791 counter++;
792 }
793 if (counter == 20)
794 {
795 pinMode(US_ECHO, OUTPUT);
796 digitalWrite(US_ECHO, HIGH);
797 delayMicroseconds(10);
798 digitalWrite(US_ECHO, LOW);
799 pinMode(US_ECHO, INPUT);
800 delayMicroseconds(5);
801 distance = 10000;
802 }
803 }
804
805 int16_t measure_distance()
806 {
807 start_distance_measurement();
808 wait_for_distance_measurement_to_complete();
809 return distance;
810 }
