Difference between revisions of "Otto - riadiaci program v.2"
Line 243: | Line 243: | ||
void menu_command(int cmd) | void menu_command(int cmd) | ||
{ | { | ||
+ | if (cmd == 2) ahoj(); | ||
serial_println_num(cmd); | serial_println_num(cmd); | ||
} | } | ||
Line 421: | Line 422: | ||
int z; | int z; | ||
do { | do { | ||
− | z = | + | z = read_char(); |
− | if (z == '#') while (z != 13) z = | + | if (z == '#') while (z != 13) z = read_char(); |
} while ((z < '0') || (z > '9')); | } while ((z < '0') || (z > '9')); | ||
while ((z >= '0') && (z <= '9')) | while ((z >= '0') && (z <= '9')) | ||
Line 428: | Line 429: | ||
num *= 10; | num *= 10; | ||
num += (z - '0'); | num += (z - '0'); | ||
− | do { z = | + | do { z = read_char(); if (z == -1) delayMicroseconds(10); } while (z < 0); |
} | } | ||
return num; | return num; |
Revision as of 09:20, 3 August 2018
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 | Kalibracia |
---|---|---|---|
a/q | ľavá ruka | 10 | 4 |
;/p | pravá ruka | 11 | 5 |
z/x | ľavá noha | 9 | 3 |
,/. | pravá noha | 6 | 2 |
d/c | ľavá päta | 5 | 1 |
k/m | pravá päta | 3 | 0 |
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 int count;
147 while ((millis() - tm < 3000) && (d < 15) & (count < 10))
148 {
149 delay(10);
150 d = measure_distance();
151 if (d == 10000) count++;
152 else count = 0;
153 }
154 if (millis() - tm >= 3000)
155 ultrasonic_menu();
156 }
157
158 void ultrasonic_menu()
159 {
160 int selection = 0;
161 tone(SIRENA, 880, 200);
162
163 do {
164 int count = 0;
165 do {
166 int32_t d = measure_distance();
167 if (d == 10000) continue;
168 if (d >= 20) count++;
169 else count = 0;
170 delay(10);
171 } while (!serial_available() && !Serial.available() && (count < 20));
172
173 tone(SIRENA, 440, 200);
174 uint32_t tm = millis();
175 while ((measure_distance() > 15) && (millis() - tm < 1500) && !serial_available() && !Serial.available()) delay(10);
176 if (millis() - tm >= 1500)
177 {
178 tone(SIRENA, 2000, 50);
179 menu_command(selection);
180 return;
181 }
182 selection++;
183 for (int i = 0; i < selection; i++)
184 {
185 tone(SIRENA, 1261, 50);
186 delay(250);
187 }
188 } while (!serial_available() && !Serial.available());
189 while (serial_available()) serial_read();
190 while (Serial.available()) Serial.read();
191 }
192
193 void menu_command(int cmd)
194 {
195 if (cmd == 2) ahoj();
196 serial_println_num(cmd);
197 }
198
199 void precitaj_kalibraciu_z_EEPROM()
200 {
201 uint8_t value = EEPROM.read(1);
202 if (value != '~') return;
203 for (int i = 2; i < 8; i++)
204 prednastavena_kalibracia[i - 2] = EEPROM.read(i);
205 for (int i = 0; i < 6; i++)
206 dolny_limit[i] = EEPROM.read(i + 9);
207 for (int i = 0; i < 6; i++)
208 horny_limit[i] = EEPROM.read(i + 15);
209 }
210
211 char read_char()
212 {
213 while (!serial_available() && !Serial.available());
214 if (serial_available()) return serial_read();
215 else return Serial.read();
216 }
217
218 void zapis_kalibraciu_do_EEPROM()
219 {
220 serial_print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
221 char odpoved = read_char();
222 serial_println_char(odpoved);
223 if (odpoved == 'Y')
224 {
225 EEPROM.write(1, '~');
226 for (int i = 2; i < 8; i++)
227 EEPROM.write(i, kalib[i - 2]);
228 for (int i = 0; i < 6; i++)
229 EEPROM.write(9 + i, dolny_limit[i]);
230 for (int i = 0; i < 6; i++)
231 EEPROM.write(15 + i, horny_limit[i]);
232 serial_println("ok");
233 }
234 }
235
236 void pipni()
237 {
238 tone(SIRENA, 1568, 50);
239 delay(100);
240 tone(SIRENA, 1357, 50);
241 }
242
243 void ruky()
244 {
245 int odloz_krok = krok;
246 delay(500);
247 krok = 90;
248 pohyb(SERVO_LAVA_RUKA);
249 pohyb(SERVO_PRAVA_RUKA);
250 delay(1000);
251 krok = 180;
252 pohyb(-SERVO_LAVA_RUKA);
253 pohyb(-SERVO_PRAVA_RUKA);
254 delay(1000);
255 krok = odloz_krok;
256 pipni();
257 }
258
259 void ruky2()
260 {
261 int odloz_krok = krok;
262 delay(500);
263 krok = 180;
264 pohyb(SERVO_LAVA_RUKA);
265 pohyb(SERVO_PRAVA_RUKA);
266 delay(1000);
267 krok = 90;
268 pohyb(-SERVO_LAVA_RUKA);
269 pohyb(-SERVO_PRAVA_RUKA);
270 delay(1000);
271 krok = odloz_krok;
272 pipni();
273 }
274
275 void ahoj()
276 {
277 tone(SIRENA, 1568, 50);
278 delay(70);
279 tone(SIRENA, 1175, 30);
280 delay(50);
281 tone(SIRENA, 880, 30);
282 delay(50);
283 tone(SIRENA, 1047, 50);
284 delay(70);
285 tone(SIRENA, 1245, 30);
286 delay(150);
287 tone(SIRENA, 1568, 50);
288 delay(100);
289 if (random(10) > 4) tone(SIRENA, 1357, 50);
290 else tone(SIRENA, 1047, 50);
291 }
292
293 void nastav_koncatinu(int8_t servo, uint8_t poloha)
294 {
295 int8_t srv = (servo > 0)?servo:-servo;
296 srv--;
297 poloha += kalib[srv] - 90;
298 if (poloha > 180) poloha = 180;
299 if (poloha < 0) poloha = 0;
300 stav[srv] = poloha;
301 s[srv].write(stav[srv]);
302 }
303
304 void pohyb(int8_t servo)
305 {
306 int8_t srv = (servo > 0)?servo:-servo;
307 srv--;
308 if (servo_invertovane[srv]) servo = -servo;
309 if (servo > 0)
310 {
311 if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
312 else stav[srv] = horny_limit[srv];
313 s[srv].write(stav[srv]);
314 }
315 else if (servo < 0)
316 {
317 if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok;
318 else stav[srv] = dolny_limit[srv];
319 s[srv].write(stav[srv]);
320 }
321 }
322
323 uint8_t pohyb_znakom(char z)
324 {
325 for (int i = 0; i < 12; i++)
326 {
327 if (z == znaky_zmien[i])
328 {
329 int8_t servo = zmeny[i];
330 pohyb(servo);
331 }
332 }
333 }
334
335 void kombinacia1()
336 {
337 pohyb(SERVO_LAVA_NOHA);
338 pohyb(-SERVO_PRAVA_PATA);
339 }
340
341 void kombinacia2()
342 {
343 pohyb(SERVO_PRAVA_NOHA);
344 pohyb(-SERVO_LAVA_PATA);
345 }
346
347 void kombinacia3()
348 {
349 pohyb(SERVO_LAVA_RUKA);
350 pohyb(SERVO_PRAVA_RUKA);
351 }
352
353 void kombinacia4()
354 {
355 pohyb(-SERVO_LAVA_RUKA);
356 pohyb(-SERVO_PRAVA_RUKA);
357 }
358
359 int pohyb_kombinacia(char z)
360 {
361 if (z == '3') kombinacia1();
362 else if (z == '4') kombinacia2();
363 else if (z == '5') kombinacia3();
364 else if (z == '6') kombinacia4();
365 else return 0;
366 return 1;
367 }
368
369 int nacitajCislo()
370 {
371 int num = 0;
372 int z;
373 do {
374 z = read_char();
375 if (z == '#') while (z != 13) z = read_char();
376 } while ((z < '0') || (z > '9'));
377 while ((z >= '0') && (z <= '9'))
378 {
379 num *= 10;
380 num += (z - '0');
381 do { z = read_char(); if (z == -1) delayMicroseconds(10); } while (z < 0);
382 }
383 return num;
384 }
385
386 void nacitaj_choreografiu()
387 {
388 ch_len = 0;
389 int tm;
390 do {
391 tm = nacitajCislo();
392 ch_time[ch_len] = tm;
393 ch_servo[ch_len] = nacitajCislo();
394 ch_val[ch_len] = nacitajCislo();
395 ch_len++;
396 if (ch_len == CHOREO_LEN) break;
397 } while (tm > 0);
398 pipni();
399 }
400
401 void vypis_choreografiu()
402 {
403 for (int i = 0; i < ch_len; i++)
404 {
405 serial_print_num(ch_time[i]);
406 serial_print(" ");
407 serial_print_num(ch_servo[i]);
408 serial_print(" ");
409 serial_println_num(ch_val[i]);
410 }
411 pipni();
412 }
413
414 void zatancuj_choreografiu()
415 {
416 for (int i = 0; i < ch_len - 1; i++)
417 {
418 delay(ch_time[i]);
419 nastav_koncatinu(ch_servo[i], ch_val[i]);
420 }
421 if (ch_len > 0) stav[ch_servo[ch_len - 1]] = ch_val[ch_len - 1];
422 pipni();
423 }
424
425 void reset()
426 {
427 for (int i = 0; i < 6; i++)
428 {
429 stav[i] = kalib[i];
430 s[i].write(kalib[i]);
431 }
432 pipni();
433 }
434
435 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
436 {
437 serial_print(" (+/-/ENTER): ");
438 serial_println_num(hodnota);
439 s[servo].write(hodnota);
440 char z;
441 do {
442 z = read_char();
443 if ((z == '+') && (hodnota < 180)) hodnota++;
444 else if ((z == '-') && (hodnota > 0)) hodnota--;
445 if ((z == '+') || (z == '-'))
446 {
447 serial_print_num(hodnota); serial_print_char('\r');
448 s[servo].write(hodnota);
449 }
450 } while (z != 13);
451 return hodnota;
452 }
453
454 void kalibruj()
455 {
456 for (int i = 0; i < 6; i++)
457 {
458 serial_print_num(i);
459 kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
460 serial_print_num(i);
461 serial_print(": ");
462 serial_println_num(kalib[i]);
463 }
464 for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
465 serial_println("ok");
466 pipni();
467 }
468
469 void nastav_limity()
470 {
471 for (int i = 0; i < 6; i++)
472 {
473 serial_print_num(i);
474 serial_print("dolny");
475 dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
476 serial_print_num(i);
477 serial_print(" dolny: ");
478 serial_println_num(dolny_limit[i]);
479 s[i].write(kalib[i]);
480
481 serial_print_num(i);
482 serial_print("horny");
483 horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
484 serial_print_num(i);
485 serial_print(" horny: ");
486 serial_println_num(horny_limit[i]);
487 s[i].write(kalib[i]);
488 }
489 for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print("-"); serial_print_num(horny_limit[i]); serial_print(" "); }
490 serial_println("ok");
491 pipni();
492 }
493
494 void vypis_kalibraciu()
495 {
496 serial_print("stredy: ");
497 for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
498 serial_println();
499 serial_print("dolny limit: ");
500 for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print(" "); }
501 serial_println();
502 serial_print("horny limit: ");
503 for (int i = 0; i < 6; i++) { serial_print_num(horny_limit[i]); serial_print(" "); }
504 serial_println();
505 }
506
507 void nacitaj_kalibraciu()
508 {
509 int tm;
510 for (int i = 0; i < 6; i++)
511 kalib[i] = nacitajCislo();
512 vypis_kalibraciu();
513 serial_println("ok");
514 pipni();
515 }
516
517 void zvys_krok()
518 {
519 if (krok < 180) krok++;
520 serial_print("krok: ");
521 serial_println_num(krok);
522 }
523
524 void zniz_krok()
525 {
526 if (krok > 0) krok--;
527 serial_print("krok: ");
528 serial_println_num(krok);
529 }
530
531 // nasleduje softverova implementacia serioveho portu
532 #define SERIAL_STATE_IDLE 0
533 #define SERIAL_STATE_RECEIVING 1
534 #define SERIAL_BUFFER_LENGTH 20
535
536 static volatile uint8_t serial_state;
537 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
538 static volatile uint8_t serial_buf_wp, serial_buf_rp;
539
540 static volatile uint8_t receiving_byte;
541
542 static volatile uint32_t time_startbit_noticed;
543 static volatile uint8_t next_bit_order;
544 static volatile uint8_t waiting_stop_bit;
545 static uint16_t one_byte_duration;
546 static uint16_t one_bit_duration;
547 static uint16_t one_bit_write_duration;
548 static uint16_t half_of_one_bit_duration;
549
550 void init_serial(uint32_t baud_rate)
551 {
552 pinMode(2, INPUT);
553 pinMode(4, OUTPUT);
554
555 serial_state = SERIAL_STATE_IDLE;
556
557 one_byte_duration = 9500000 / baud_rate;
558 one_bit_duration = 1000000 / baud_rate;
559 one_bit_write_duration = one_bit_duration - 1;
560 half_of_one_bit_duration = 500000 / baud_rate;
561
562 PCMSK2 |= 4; //PCINT18;
563 PCIFR &= ~4; //PCIF2;
564 PCICR |= 4; // PCIE2;
565 }
566
567 ISR(PCINT2_vect)
568 {
569 uint32_t tm = micros();
570 if (serial_state == SERIAL_STATE_IDLE)
571 {
572 time_startbit_noticed = tm;
573 serial_state = SERIAL_STATE_RECEIVING;
574 receiving_byte = 0xFF;
575 next_bit_order = 0;
576 }
577 else if (tm - time_startbit_noticed > one_byte_duration)
578 {
579 serial_buffer[serial_buf_wp] = receiving_byte;
580 serial_buf_wp++;
581 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
582 time_startbit_noticed = tm;
583 receiving_byte = 0xFF;
584 next_bit_order = 0;
585 }
586 else if (PIND & 4)
587 {
588 int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
589 while (next_bit_order < new_next_bit_order)
590 {
591 receiving_byte &= ~(1 << next_bit_order);
592 next_bit_order++;
593 }
594 if (next_bit_order == 8)
595 {
596 serial_buffer[serial_buf_wp] = receiving_byte;
597 serial_buf_wp++;
598 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
599 serial_state = SERIAL_STATE_IDLE;
600 }
601 } else
602 next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
603 }
604
605 uint8_t serial_available()
606 {
607 cli();
608 if (serial_buf_rp != serial_buf_wp)
609 {
610 sei();
611 return 1;
612 }
613 if (serial_state == SERIAL_STATE_RECEIVING)
614 {
615 uint32_t tm = micros();
616 if (tm - time_startbit_noticed > one_byte_duration)
617 {
618 serial_state = SERIAL_STATE_IDLE;
619 serial_buffer[serial_buf_wp] = receiving_byte;
620 serial_buf_wp++;
621 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
622 sei();
623 return 1;
624 }
625 }
626 sei();
627 return 0;
628 }
629
630 int16_t serial_read()
631 {
632 cli();
633 if (serial_buf_rp != serial_buf_wp)
634 {
635 uint8_t ch = serial_buffer[serial_buf_rp];
636 serial_buf_rp++;
637 if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
638 sei();
639 return ch;
640 }
641
642 if (serial_state == SERIAL_STATE_RECEIVING)
643 {
644 uint32_t tm = micros();
645 if (tm - time_startbit_noticed > one_byte_duration)
646 {
647 uint8_t ch = receiving_byte;
648 serial_state = SERIAL_STATE_IDLE;
649 sei();
650 return ch;
651 }
652 }
653 sei();
654 return -1;
655 }
656
657 void serial_write(uint8_t ch)
658 {
659 #ifdef ECHO_BT_TO_USB
660 Serial.print((char)ch);
661 #endif
662 PORTD &= ~16;
663 delayMicroseconds(one_bit_write_duration);
664 for (uint8_t i = 0; i < 8; i++)
665 {
666 if (ch & 1) PORTD |= 16;
667 else PORTD &= ~16;
668 ch >>= 1;
669 delayMicroseconds(one_bit_write_duration);
670 }
671 PORTD |= 16;
672 delayMicroseconds(one_bit_write_duration);
673 delayMicroseconds(one_bit_write_duration);
674 delayMicroseconds(one_bit_write_duration);
675 delayMicroseconds(one_bit_write_duration);
676 delayMicroseconds(one_bit_write_duration);
677 }
678
679 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
680 {
681 uint16_t len;
682 uint16_t ch;
683 do {
684 ch = serial_read();
685 if (ch == 13) continue;
686 } while (ch == -1);
687
688 do {
689 if ((ch != 13) && (ch != 10) && (ch != -1))
690 {
691 *(ln++) = ch;
692 max_length--;
693 len++;
694 }
695 ch = serial_read();
696 } while ((ch != 13) && max_length);
697 *ln = 0;
698 return len;
699 }
700
701 void serial_print_num(int32_t number)
702 {
703 if (number < 0)
704 {
705 serial_write('-');
706 number = -number;
707 }
708 int32_t rad = 1;
709 while (number / rad) rad *= 10;
710 if (number > 0) rad /= 10;
711 while (rad)
712 {
713 serial_write((char)('0' + (number / rad)));
714 number -= (number / rad) * rad;
715 rad /= 10;
716 }
717 }
718
719 void serial_print_char(char ch)
720 {
721 serial_write(ch);
722 }
723
724 void serial_print(const uint8_t *str)
725 {
726 while (*str) serial_write(*(str++));
727 }
728
729 void serial_println(const uint8_t *str)
730 {
731 serial_print(str);
732 serial_write(13);
733 serial_write(10);
734 }
735
736 void serial_println_num(int32_t number)
737 {
738 serial_print_num(number);
739 serial_println();
740 }
741
742 void serial_println_char(char ch)
743 {
744 serial_write(ch);
745 serial_println();
746 }
747
748 void serial_println()
749 {
750 serial_write(13);
751 serial_write(10);
752 }
753
754 // nasleduje citanie z utltazvukoveho senzora
755
756 static volatile uint32_t pulse_start;
757 static volatile int16_t distance;
758 static volatile uint8_t new_distance;
759
760 void init_ultrasonic()
761 {
762 pinMode(US_ECHO, INPUT);
763 pinMode(US_TRIG, OUTPUT);
764
765 PCMSK0 |= 1; //PCINT0;
766 PCIFR &= ~1; //PCIF0;
767 PCICR |= 1; // PCIE0;
768 }
769
770 ISR(PCINT0_vect)
771 {
772 if (PINB & 1) pulse_start = micros();
773 else
774 {
775 distance = (int16_t)((micros() - pulse_start) / 58);
776 new_distance = 1;
777 }
778 }
779
780 void start_distance_measurement()
781 {
782 distance = 10000;
783 new_distance = 0;
784 digitalWrite(US_TRIG, HIGH);
785 delayMicroseconds(10);
786 digitalWrite(US_TRIG, LOW);
787 }
788
789 void wait_for_distance_measurement_to_complete()
790 {
791 uint8_t counter = 0;
792 while ((counter < 20) && !new_distance)
793 {
794 delay(1);
795 counter++;
796 }
797 if (counter == 20)
798 {
799 pinMode(US_ECHO, OUTPUT);
800 digitalWrite(US_ECHO, HIGH);
801 delayMicroseconds(10);
802 digitalWrite(US_ECHO, LOW);
803 pinMode(US_ECHO, INPUT);
804 delayMicroseconds(5);
805 distance = 10000;
806 }
807 }
808
809 int16_t measure_distance()
810 {
811 start_distance_measurement();
812 wait_for_distance_measurement_to_complete();
813 return distance;
814 }