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