Otto - riadiaci program v.4
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.
Pripojenie na vývody Arduina:
signál | pin |
---|---|
ľavá ruka | 10 |
pravá ruka | 11 |
ľavá noha | 9 |
pravá noha | 6 |
ľavá päta | 5 |
pravá päta | 3 |
TXD BlueTooth | 2 |
RXD BlueTooth | 4 |
TRIG Ultrazvuk | 7 |
ECHO Ultrazvuk | 8 |
Sirena | 12 |
Číslovanie serva v choreografii:
servo | číslo serva v choreografii |
---|---|
ľavá ruka | 5 |
pravá ruka | 6 |
ľavá noha | 4 |
pravá noha | 3 |
ľavá päta | 2 |
pravá päta | 1 |
klaves | servo/funkcia |
---|---|
a/q | ľavá ruka |
;/p | pravá ruka |
z/x | ľavá noha |
,/. | pravá noha |
d/c | ľavá päta |
k/m | pravá päta |
1/9 | zníž/zvýš rýchlosť pohybu |
H | kalibrovať strednú polohu servomotorov
|
J | kalibrovať limity pre všetky stupne voľnosti (len pre priame riadenie) |
G | zobrazenie celej kalibrácie |
E | celú kalibráciu je možné zapísať do trvalej pamäte EEPROM (používať opatrne! - dá sa prepísať najviac 100000-krát), po zapnutí sa automaticky načíta, treba potvrdiť veľkým Y |
minus | zvukový pozdrav |
R | ruky hore-dole |
@ | načítanie choreografie, pozri príklady a formát |
? | zobrazí načítanú choreografiu (kontrola po @) |
t | zatancuje načítanú choreografiu |
U | testuje ultrazvuk |
S | zobrazí aktuálnu polohu servomotorov (vhodné pri laden |
medzera | reset všetkých motorov do strednej polohy |
3,...,6 | zmiešaný pohyb (treba nastaviť vo funkciách kombinacia1() - kombinacia4()) |
v,b,n,j,h,g,f,s,l,o,i,u,y,r,e,w | zvukové efekty 1-16 |
TAB,*,/,+ | zahrá melódiu 1-4 |
W | zapísať choreografiu do EEPROM (opatrne) |
C | načítať choreografiu z EEPROM |
X | formátovať EEPROM na zápis choreografií (opatrne) |
V | vypni/zapni režim nudy |
A | automatický štart choreografie po zapnutí |
Pozrite si (a skopírujte si) príklady choreografií: Otto - príklady choreografií
Download: otto4.ino
1 #include <Servo.h>
2 #include <EEPROM.h>
3 #include <avr/pgmspace.h>
4
5 #define ECHO_BT_TO_USB 1
6
7 #define US_TRIG 7
8 #define US_ECHO 8
9
10 #define BT_RX 2
11 #define BT_TX 4
12
13 #define SIRENA 12
14
15 //maximalna dlzka choreografie
16 #define CHOREO_LEN 200
17
18 #define CAS_OTTO_SA_ZACNE_NUDIT 9000
19
20 // cisla pinov, kde su zapojene servo motory
21 #define PIN_SERVO_LAVA_RUKA 10
22 #define PIN_SERVO_PRAVA_RUKA 11
23 #define PIN_SERVO_LAVA_NOHA 9
24 #define PIN_SERVO_PRAVA_NOHA 6
25 #define PIN_SERVO_LAVA_PATA 5
26 #define PIN_SERVO_PRAVA_PATA 3
27
28 #define S1 3
29 #define S2 5
30 #define S3 6
31 #define S4 9
32 #define S5 10
33 #define S6 11
34
35 // tu su serva cislovane 1-6
36 #define SERVO_LAVA_RUKA 5
37 #define SERVO_PRAVA_RUKA 6
38 #define SERVO_LAVA_NOHA 4
39 #define SERVO_PRAVA_NOHA 3
40 #define SERVO_LAVA_PATA 2
41 #define SERVO_PRAVA_PATA 1
42
43 // ak su niektore serva naopak, je tu jednotka
44 uint8_t servo_invertovane[6] = {0, 0, 0, 0, 0, 0};
45
46 // znaky, ktorymi sa ovladaju jednotlive stupne volnosti
47 char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' };
48 // co robia jednotlive znaky (znamienko urcuje smer)
49 int8_t zmeny[] = {-SERVO_LAVA_RUKA, SERVO_LAVA_RUKA,
50 SERVO_PRAVA_RUKA, -SERVO_PRAVA_RUKA,
51 -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA,
52 -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA,
53 SERVO_LAVA_PATA, -SERVO_LAVA_PATA,
54 -SERVO_PRAVA_PATA, SERVO_PRAVA_PATA
55 };
56
57 // sem si mozno ulozit svoju kalibraciu
58 uint8_t prednastavena_kalibracia[] = { 90, 90, 90, 90, 90, 90 };
59
60 uint8_t dolny_limit[] = {0, 0, 0, 0, 0, 0, 0, 0};
61 uint8_t horny_limit[] = {180, 180, 180, 180, 180, 180};
62
63 Servo s[6];
64
65 uint16_t ch_time[CHOREO_LEN];
66 uint8_t ch_servo[CHOREO_LEN];
67 uint8_t ch_val[CHOREO_LEN];
68 int ch_len;
69 uint8_t kalib[6];
70 int stav[6];
71 int krok;
72 uint8_t spomalenie;
73 uint8_t auto_start;
74
75 static volatile int16_t distance;
76 static uint32_t cas_ked_naposledy_stlacil;
77 static uint8_t vypni_nudu;
78
79 void setup() {
80 Serial.begin(9600);
81 init_tone2();
82 init_serial(57600);
83 init_ultrasonic();
84
85 randomSeed(analogRead(1));
86 s[0].attach(PIN_SERVO_PRAVA_PATA);
87 s[1].attach(PIN_SERVO_LAVA_PATA);
88 s[2].attach(PIN_SERVO_PRAVA_NOHA);
89 s[3].attach(PIN_SERVO_LAVA_NOHA);
90 s[4].attach(PIN_SERVO_LAVA_RUKA);
91 s[5].attach(PIN_SERVO_PRAVA_RUKA);
92 precitaj_kalibraciu_z_EEPROM();
93 for (int i = 0; i < 6; i++)
94 {
95 kalib[i] = prednastavena_kalibracia[i];
96 stav[i] = kalib[i];
97 s[i].write(stav[i]);
98 }
99 ch_len = 0;
100 krok = 7;
101 vypni_nudu = 0;
102 spomalenie = 6;
103 ahoj();
104 ruky();
105 delay(100);
106 serial_println_flash(PSTR("\r\n Otto DTDT"));
107 if (auto_start > 0)
108 {
109 delay(7000);
110 while (serial_available()) serial_read();
111 while (Serial.available()) Serial.read();
112 precitaj_choreografiu_z_EEPROM(auto_start);
113 zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
114 }
115 cas_ked_naposledy_stlacil = millis();
116 }
117
118 void loop() {
119 char z = -1;
120 if (serial_available()) z = serial_read();
121 #ifdef ECHO_BT_TO_USB
122 if (Serial.available()) z = Serial.read();
123 #endif
124
125 if (z != -1)
126 {
127 cas_ked_naposledy_stlacil = millis();
128 if (pohyb_znakom(z)) return;
129 else if (pohyb_kombinacia(z)) return;
130 else if (vydaj_zvukovy_efekt(z)) return;
131 else if (ma_zahrat_melodiu(z)) return;
132 else if (z == '@') nacitaj_choreografiu();
133 else if (z == '?') vypis_choreografiu();
134 else if (z == 't') zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
135 else if (z == '-') ahoj();
136 else if (z == ' ') reset();
137 else if (z == 'H') kalibruj();
138 else if (z == 'J') nastav_limity();
139 else if (z == 'G') vypis_kalibraciu();
140 else if (z == 'L') nacitaj_kalibraciu();
141 else if (z == 'E') zapis_kalibraciu_do_EEPROM();
142 else if (z == 'R') ruky();
143 else if (z == '9') zvys_krok();
144 else if (z == '1') zniz_krok();
145 else if (z == '8') zvys_spomalenie();
146 else if (z == '7') zniz_spomalenie();
147 else if (z == 'U') test_ultrazvuk();
148 else if (z == 'S') zobraz_stav();
149 else if (z == 'W') skus_zapisat_choreografiu_do_EEPROM();
150 else if (z == 'X') skus_naformatovat_EEPROM_choreografie();
151 else if (z == 'C') skus_nacitat_choreografiu_z_EEPROM();
152 else if (z == 'A') nastav_automaticky_start();
153 else if (z == 'V') prepni_nudu();
154 }
155 int16_t d = measure_distance();
156 if (d < 10) menu_ultrasonic_request();
157
158 skus_ci_sa_nenudi();
159 }
160
161 void nastav_automaticky_start()
162 {
163 serial_print_flash(PSTR("Automaticky start [1,2,3, 0=vypni]:"));
164 char z;
165 do { z = read_char(); } while ((z < '0') || (z > '3'));
166 if (z == '0') EEPROM.write(1, '~');
167 else EEPROM.write(1, z);
168 serial_println_char(z);
169 }
170
171 void prepni_nudu()
172 {
173 vypni_nudu ^= 1;
174 serial_print_flash(PSTR("ticho:"));
175 serial_println_num(vypni_nudu);
176 }
177
178 void skus_ci_sa_nenudi()
179 {
180 if ((millis() - cas_ked_naposledy_stlacil > CAS_OTTO_SA_ZACNE_NUDIT) && !vypni_nudu)
181 {
182 uint8_t rnd = random(21);
183 switch (rnd)
184 {
185 case 0: zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len); break;
186 case 1: zahraj_melodiu(1); break;
187 case 2: zahraj_melodiu(2); break;
188 case 3: zahraj_melodiu(3); break;
189 case 4: zahraj_melodiu(4); break;
190 case 5: for (int i = 0; i < 20; i++)
191 {
192 rnd = random(16) + 1;
193 zvukovy_efekt(rnd);
194 }
195 break;
196 default: zvukovy_efekt(rnd - 5); break;
197 }
198 cas_ked_naposledy_stlacil = millis();
199 }
200 }
201
202 const uint8_t klavesy_efektov[] PROGMEM = {'v', 'b', 'n', 'j', 'h', 'g', 'f', 's', 'l', 'o', 'i', 'u', 'y', 'r', 'e', 'w'};
203 #define POCET_KLAVES_EFEKTOV 16
204
205 uint8_t vydaj_zvukovy_efekt(char z)
206 {
207 for (int i = 0; i < POCET_KLAVES_EFEKTOV; i++)
208 if (z == pgm_read_byte(klavesy_efektov + i))
209 {
210 zvukovy_efekt(i + 1);
211 return 1;
212 }
213 return 0;
214 }
215
216 uint8_t ma_zahrat_melodiu(char z)
217 {
218 if (z == 9) zahraj_melodiu(1);
219 else if (z == '*') zahraj_melodiu(2);
220 else if (z == '/') zahraj_melodiu(3);
221 else if (z == '+') zahraj_melodiu(4);
222 else if (z == 27) zastav_melodiu();
223 }
224
225 void efekt1()
226 {
227 uint16_t fre = 100;
228 for (int i = 0; i < 200; i++)
229 {
230 tone2(fre, 2);
231 fre += 10;
232 delay(2);
233 }
234 }
235
236 void efekt2()
237 {
238 uint16_t fre = 2060;
239 for (int i = 0; i < 200; i++)
240 {
241 tone2(fre, 2);
242 fre -= 10;
243 delay(2);
244 }
245 }
246
247 void efekt3()
248 {
249 uint16_t fre;
250 for (int i = 0; i < 200; i++)
251 {
252 fre = random(3000) + 20;
253 tone2(fre, 2);
254 delay(2);
255 }
256 }
257
258 void efekt4()
259 {
260 uint16_t fre = 100;
261 for (int i = 0; i < 200; i++)
262 {
263 (i & 1) ? tone2(fre, 2) : tone2(2100 - fre, 2);
264 fre += 10;
265 delay(2);
266 }
267 }
268
269 void efekt5()
270 {
271 uint16_t fre = 100;
272 for (int i = 0; i < 100; i++)
273 {
274 (i & 1) ? tone2(fre, 4) : tone2(2100 - fre, 4);
275 fre += 20;
276 delay(4);
277 }
278 }
279
280 void efekt6()
281 {
282 uint16_t d = 12;
283 for (int i = 0; i < 20; i++)
284 {
285 tone2(1760, 10);
286 delay(d);
287 d += 3;
288 }
289 }
290
291 void efekt7()
292 {
293 for (int i = 0; i < 40; i++)
294 {
295 if (i & 1) tone2(1760, 10);
296 delay(10);
297 }
298 }
299
300 void efekt8()
301 {
302 uint16_t d = 72;
303 for (int i = 0; i < 20; i++)
304 {
305 tone2(1760, 10);
306 delay(d);
307 d -= 3;
308 }
309 }
310
311 void efekt9()
312 {
313 float fre = 3500;
314 while (fre > 30)
315 {
316 tone2((uint16_t)fre, 2);
317 fre *= 0.97;
318 delay(2);
319 }
320 }
321
322 void efekt10()
323 {
324 float fre = 30;
325 while (fre < 3000)
326 {
327 tone2((uint16_t)fre, 2);
328 fre *= 1.03;
329 delay(2);
330 }
331 }
332
333 void efekt11()
334 {
335 uint16_t fre = 3500;
336 uint16_t d = 42;
337 for (int i = 0; i < 20; i++)
338 {
339 tone2(fre, d);
340 fre -= 165;
341 delay(d);
342 d -= 2;
343 }
344 }
345
346 void efekt12()
347 {
348 float fre = 110;
349 uint8_t d = 42;
350 for (int i = 0; i < 20; i++)
351 {
352 tone2(fre, d);
353 fre += 165;
354 delay(d);
355 d -= 2;
356 }
357 }
358
359 void efekt13()
360 {
361 uint16_t fre = 3400;
362 uint8_t d = 200;
363 uint8_t delta = 1;
364 for (int i = 0; i < 20; i++)
365 {
366 tone2(fre, d);
367 fre -= 165;
368 delay(d);
369 d -= delta;
370 delta++;
371 }
372 tone2(110, 1000);
373 delay(1000);
374 }
375
376 void efekt14()
377 {
378 uint16_t fre = 880;
379 int16_t d = 20;
380 for (int i = 0; i < 20; i++)
381 {
382 tone2(fre, d);
383 delay(d);
384 fre += random(50) - 25;
385 d += random(10) - 5;
386 if (d < 0) d = 1;
387 }
388 }
389
390 void efekt15()
391 {
392 uint16_t fre = 440;
393 int16_t d = 20;
394 for (int i = 0; i < 20; i++)
395 {
396 tone2(fre, d);
397 delay(d);
398 fre += random(25) - 12;
399 d += random(10) - 5;
400 if (d < 0) d = 1;
401 }
402 }
403
404 void efekt16()
405 {
406 uint16_t fre = 1760;
407 int16_t d = 20;
408 for (int i = 0; i < 20; i++)
409 {
410 tone2(fre, d);
411 delay(d);
412 fre += random(100) - 50;
413 d += random(10) - 5;
414 if (d < 0) d = 1;
415 }
416 }
417
418 void zvukovy_efekt(uint8_t cislo)
419 {
420 switch (cislo) {
421 case 1: efekt1(); break;
422 case 2: efekt2(); break;
423 case 3: efekt3(); break;
424 case 4: efekt4(); break;
425 case 5: efekt5(); break;
426 case 6: efekt6(); break;
427 case 7: efekt7(); break;
428 case 8: efekt8(); break;
429 case 9: efekt9(); break;
430 case 10: efekt10(); break;
431 case 11: efekt11(); break;
432 case 12: efekt12(); break;
433 case 13: efekt13(); break;
434 case 14: efekt14(); break;
435 case 15: efekt15(); break;
436 case 16: efekt16(); break;
437 }
438 }
439
440 void test_ultrazvuk()
441 {
442 int i = 0;
443 while ((serial_available() == 0) && (Serial.available() == 0))
444 {
445 serial_println_num(measure_distance());
446 delay(100);
447 }
448 serial_read();
449 }
450
451 void menu_ultrasonic_request()
452 {
453 uint32_t tm = millis();
454 int d = measure_distance();
455 int count = 0;
456 int count2 = 0;
457 while ((millis() - tm < 1500) && ((d < 15) || (count2 < 5)) && (count < 10))
458 {
459 delay(10);
460 d = measure_distance();
461 if (d == 10000) count++;
462 else count = 0;
463 if (d >= 15) count2++;
464 else count2 = 0;
465 if (serial_available() || Serial.available()) return;
466 }
467 if (millis() - tm >= 1500)
468 {
469 ultrasonic_menu();
470 cas_ked_naposledy_stlacil = millis();
471 }
472 }
473
474 void ultrasonic_menu()
475 {
476 int selection = 0;
477 tone2( 880, 200);
478
479 do {
480 int count = 0;
481 do {
482 int32_t d = measure_distance();
483 if (d == 10000) continue;
484 if (d >= 20) count++;
485 else count = 0;
486 delay(10);
487 } while (!serial_available() && !Serial.available() && (count < 20));
488
489 tone2( 440, 200);
490 uint32_t tm = millis();
491 while ((measure_distance() > 15) && (millis() - tm < 1500) && !serial_available() && !Serial.available()) delay(10);
492 if (millis() - tm >= 1500)
493 {
494 tone2( 2000, 50);
495 menu_command(selection);
496 return;
497 }
498 selection++;
499 for (int i = 0; i < selection; i++)
500 {
501 tone2( 1261, 50);
502 delay(250);
503 }
504 } while (!serial_available() && !Serial.available());
505 while (serial_available()) serial_read();
506 while (Serial.available()) Serial.read();
507 }
508
509 void menu_command(int cmd)
510 {
511 if (cmd == 1) vpred();
512 if (cmd == 2) {
513 precitaj_choreografiu_z_EEPROM(1);
514 zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
515 }
516 if (cmd == 3) {
517 precitaj_choreografiu_z_EEPROM(2);
518 zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
519 }
520 if (cmd == 4) {
521 precitaj_choreografiu_z_EEPROM(3);
522 zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
523 }
524 if (cmd == 5) zahraj_melodiu(1);
525 if (cmd == 6) melodia_jedna_druhej();
526 if (cmd == 7) ahoj();
527 if (cmd == 9) zahraj_melodiu(2);
528 if (cmd == 9) zahraj_melodiu(3);
529 if (cmd == 10) zahraj_melodiu(4);
530 }
531
532 void melodia_jedna_druhej()
533 {
534 for (int i = 0; i < 2; i++)
535 {
536 tone2(262, 200);
537 delay(200);
538 tone2(330, 200);
539 delay(200);
540
541 tone2(262, 200);
542 delay(200);
543 tone2(330, 200);
544 delay(200);
545
546 tone2(392, 400);
547 delay(400);
548
549 tone2(392, 400);
550 delay(400);
551 }
552 }
553
554 // chodza pre vacsieho dreveneho robota
555 uint16_t chor_time[] = {500, 100, 1, 1, 100, 1, 1, 1, 100, 1, 1, 1, 100, 1, 100, 1, 1, 0};
556 uint8_t chor_servo[] = {11, 1, 2, 6, 4, 3, 6, 5, 1, 2, 6, 5, 3, 4, 1, 2, 9, 0};
557 uint8_t chor_val[] = {1, 48, 69, 180, 104, 104, 90, 0, 111, 146, 0, 90, 62, 69, 48, 69, 2, 0};
558 uint8_t chor_len = 18;
559
560 void vpred()
561 {
562 while ((measure_distance() > 30) && !serial_available() && !Serial.available())
563 zatancuj_choreografiu(chor_time, chor_servo, chor_val, chor_len);
564 pipni();
565 reset();
566 }
567
568 void skus_zapisat_choreografiu_do_EEPROM()
569 {
570 serial_print_flash(PSTR("Cislo? [1-3]: "));
571 char odpoved = read_char();
572 if ((odpoved < '1') || (odpoved > '3')) return;
573 serial_println_char(odpoved);
574 uint8_t cislo = odpoved - '0';
575
576 serial_print_flash(PSTR("Zapisat choreografiu do EEPROM c."));
577 serial_print_char(odpoved);
578 serial_print_flash(PSTR("? [Y/n]: "));
579 odpoved = read_char();
580 serial_println_char(odpoved);
581 if (odpoved == 'Y')
582 zapis_choreografiu_do_EEPROM(cislo);
583 }
584
585 void skus_nacitat_choreografiu_z_EEPROM()
586 {
587 serial_print_flash(PSTR("Cislo? [1-3]: "));
588 char odpoved = read_char();
589 if ((odpoved < '1') || (odpoved > '3')) return;
590 serial_println_char(odpoved);
591 uint8_t cislo = odpoved - '0';
592
593 serial_print_flash(PSTR("Precitat choreografiu z EEPROM c."));
594 serial_print_char(odpoved);
595 serial_print_flash(PSTR("? [Y/n]: "));
596 odpoved = read_char();
597 serial_println_char(odpoved);
598 if (odpoved == 'Y')
599 precitaj_choreografiu_z_EEPROM(cislo);
600 }
601
602 void skus_naformatovat_EEPROM_choreografie()
603 {
604 serial_print_flash(PSTR("Formatovat EEPROM choreografii? [Y/n]:"));
605 char odpoved = read_char();
606 serial_println_char(odpoved);
607 if (odpoved == 'Y')
608 naformatuj_EEPROM_choreografie();
609 }
610
611 //EEPROM MAP:
612 // 0: slot number where choreography 3 starts (B3. Real address = B3 x 4)
613 // 1: marker '~' indicates calibration is already stored in EEPROM and will be loaded on startup
614 // '1', '2', '3' indicate the same, but choreography 1,2, or 3 is automatically launched on startup
615 // 2-8: servo calibration (central points to be used instead of default 90)
616 // 9-14: lower limit for direct control for all 6 servos
617 // 15-20: upper limit for direct control for all 6 servos
618 // 21: number of steps in choreography 1 (L1) 0=choreography not in memory
619 // 22: number of steps in choreography 2 (L2) 0=not in memory
620 // 23: number of steps in choreography 3 (L3) 0=not in memory
621 // 24..(L1 x 4 + 23) choreography1 tuplets (uint16,uint8,uint8) x L1
622 // (1024 - L2 x 4)..1023 choreography2 same as above
623 // B3 x 4..(B3 x 4 + L3 x 4 - 1) choreography 3 same as above
624 void zapis_choreografiu_do_EEPROM(int slot)
625 {
626 uint8_t b3 = EEPROM.read(0);
627 uint8_t len1 = EEPROM.read(21);
628 uint8_t len2 = EEPROM.read(22);
629 uint8_t len3 = EEPROM.read(23);
630 uint16_t wp = 65535;
631
632 if ((len1 > CHOREO_LEN) || (len2 > CHOREO_LEN) || (len3 > CHOREO_LEN) || (b3 > 250 - len3) || ((len1 > b3) && (b3 != 0)) || (len3 + b3 > 250 - len2) || (len1 + len2 > 250))
633 b3 = len1 = len2 = len3 = 0;
634
635 if (slot == 1)
636 {
637 if (((ch_len < b3) || (len3 == 0)) && (ch_len + len2 + len3 <= 250))
638 {
639 EEPROM.write(21, ch_len);
640 wp = 24;
641 }
642 }
643 else if (slot == 2)
644 {
645 if ((250 - b3 - len3 > ch_len) && (ch_len + len1 + len3 <= 250))
646 {
647 EEPROM.write(22, ch_len);
648 wp = 1000 - ch_len * 4;
649 }
650 }
651 else if (slot == 3)
652 {
653 if (ch_len + len1 + len2 <= 250)
654 {
655 EEPROM.write(23, ch_len);
656 EEPROM.write(0, (len1 - len2) / 2 + 125);
657 wp = 4 * ((len1 - len2) / 2 + 125);
658 }
659 }
660
661 if (wp == 65535)
662 serial_println_flash(PSTR("not enough space"));
663 else
664 {
665 for (int i = 0; i < ch_len; i++)
666 {
667 EEPROM.write(wp + 4 * i, ch_time[i] & 255);
668 EEPROM.write(wp + 1 + 4 * i, ch_time[i] >> 8);
669 EEPROM.write(wp + 2 + 4 * i, ch_servo[i]);
670 EEPROM.write(wp + 3 + 4 * i, ch_val[i]);
671 }
672 serial_println_flash(PSTR("ok"));
673 }
674 }
675
676 void precitaj_choreografiu_z_EEPROM(uint8_t slot)
677 {
678 uint8_t b3 = EEPROM.read(0);
679 uint8_t len1 = EEPROM.read(21);
680 uint8_t len2 = EEPROM.read(22);
681 uint8_t len3 = EEPROM.read(23);
682 uint16_t rp = 65535;
683 if ((len1 > CHOREO_LEN) || (len2 > CHOREO_LEN) || (len3 > CHOREO_LEN) || (b3 > 250 - len3) || ((len1 > b3) && (b3 != 0)) || (len3 + b3 > 250 - len2) || (len1 + len2 > 250))
684 b3 = len1 = len2 = len3 = 0;
685
686 if (slot == 1)
687 {
688 if (len1 > 0)
689 {
690 rp = 24;
691 ch_len = len1;
692 }
693 }
694 else if (slot == 2)
695 {
696 if (len2 > 0)
697 {
698 ch_len = len2;
699 rp = 1000 - ch_len * 4;
700 }
701 }
702 else if (slot == 3)
703 {
704 if (len3 > 0)
705 {
706 rp = b3 * 4;
707 ch_len = len3;
708 }
709 }
710
711 if (rp == 65535)
712 serial_println_flash(PSTR("couldn't"));
713 else
714 {
715 for (int i = 0; i < ch_len; i++)
716 {
717 ch_time[i] = ((uint16_t)EEPROM.read(rp + 4 * i)) |
718 (((uint16_t)EEPROM.read(rp + 1 + 4 * i)) << 8);
719 ch_servo[i] = EEPROM.read(rp + 2 + 4 * i);
720 ch_val[i] = EEPROM.read(rp + 3 + 4 * i);
721 }
722 serial_println_flash(PSTR("ok"));
723 pipni();
724 }
725 }
726
727 void naformatuj_EEPROM_choreografie()
728 {
729 EEPROM.write(0, 0);
730 EEPROM.write(21, 0);
731 EEPROM.write(22, 0);
732 EEPROM.write(23, 0);
733 serial_println_flash(PSTR("ok"));
734 pipni();
735 }
736
737 void precitaj_kalibraciu_z_EEPROM()
738 {
739 uint8_t value = EEPROM.read(1);
740 if ((value != '~') && (value != '1') && (value != '2') && (value != '3')) return;
741 if (value != '~') auto_start = value - '0';
742 else auto_start = 0;
743 for (int i = 2; i < 8; i++)
744 {
745 int16_t k = EEPROM.read(i);
746 if (k > 127) servo_invertovane[i - 2] = 1;
747 else servo_invertovane[i - 2] = 0;
748 k &= 127;
749 prednastavena_kalibracia[i - 2] = k + (90-63);
750 }
751 for (int i = 0; i < 6; i++)
752 dolny_limit[i] = EEPROM.read(i + 9);
753 for (int i = 0; i < 6; i++)
754 horny_limit[i] = EEPROM.read(i + 15);
755 }
756
757 char read_char()
758 {
759 while (!serial_available() && !Serial.available());
760 if (serial_available()) return serial_read();
761 else return Serial.read();
762 }
763
764 void zapis_kalibraciu_do_EEPROM()
765 {
766 serial_print_flash(PSTR("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: "));
767 char odpoved = read_char();
768 serial_println_char(odpoved);
769 if (odpoved == 'Y')
770 {
771 char kalib_state = EEPROM.read(1);
772 if ((kalib_state == '~') || ((kalib_state >= '1') && (kalib_state <= '3')))
773 EEPROM.write(1, kalib_state);
774 else EEPROM.write(1, '~');
775 for (int i = 2; i < 8; i++)
776 {
777 int16_t k = kalib[i - 2] - (90-63);
778 if (k < 0) k = 0;
779 else if (k > 127) k = 127;
780 if (servo_invertovane[i - 2]) k += 128;
781 EEPROM.write(i, (uint8_t)k);
782 }
783 for (int i = 0; i < 6; i++)
784 EEPROM.write(9 + i, dolny_limit[i]);
785 for (int i = 0; i < 6; i++)
786 EEPROM.write(15 + i, horny_limit[i]);
787 serial_println_flash(PSTR("ok"));
788 }
789 }
790
791 void pipni()
792 {
793 tone2( 1568, 50);
794 delay(100);
795 tone2( 1357, 50);
796 }
797
798 void ruky()
799 {
800 uint8_t odloz_spomalenie = spomalenie;
801 spomalenie = 0;
802 nastav_koncatinu(5, 0);
803 nastav_koncatinu(6, 0);
804 delay(1000);
805 nastav_koncatinu(5, 90);
806 nastav_koncatinu(6, 90);
807 spomalenie = odloz_spomalenie;
808 delay(1000);
809 pipni();
810 }
811
812 void ahoj()
813 {
814 tone2( 1568, 50);
815 delay(70);
816 tone2( 1175, 30);
817 delay(50);
818 tone2( 880, 30);
819 delay(50);
820 tone2( 1047, 50);
821 delay(70);
822 tone2( 1245, 30);
823 delay(150);
824 tone2( 1568, 50);
825 delay(100);
826 if (random(10) > 4) tone2( 1357, 50);
827 else tone2( 1047, 50);
828 }
829
830 void nastav_koncatinu(int8_t srv, uint8_t poloha)
831 {
832 int8_t delta;
833 srv--;
834
835 if (servo_invertovane[srv]) poloha = 180 - poloha;
836
837 if ((int16_t)poloha + (int16_t)kalib[srv] - 90 < 0) poloha = 0;
838 else poloha += kalib[srv] - 90;
839
840 if (poloha > 180) poloha = 180;
841
842 if (spomalenie == 0) {
843 stav[srv] = poloha;
844 s[srv].write(stav[srv]);
845 }
846 else {
847 if (stav[srv] < poloha) delta = 1;
848 else delta = -1;
849 while (stav[srv] != poloha)
850 {
851 stav[srv] += delta;
852 s[srv].write(stav[srv]);
853 delay(spomalenie);
854 }
855 }
856 }
857
858 void zobraz_stav()
859 {
860 for (int i = 0; i < 6; i++)
861 {
862 serial_print_flash(PSTR("S")); serial_print_num(i + 1); serial_print_flash(PSTR(": ")); serial_println_num(stav[i] - kalib[i] + 90);
863 }
864 serial_println_flash(PSTR("---"));
865 pipni();
866 }
867
868 void pohyb(int8_t servo)
869 {
870 int8_t srv = (servo > 0) ? servo : -servo;
871 srv--;
872 if (servo_invertovane[srv]) servo = -servo;
873 if (servo > 0)
874 {
875 if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
876 else stav[srv] = horny_limit[srv];
877 s[srv].write(stav[srv]);
878 }
879 else if (servo < 0)
880 {
881 if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok;
882 else stav[srv] = dolny_limit[srv];
883 s[srv].write(stav[srv]);
884 }
885 }
886
887 uint8_t pohyb_znakom(char z)
888 {
889 for (int i = 0; i < 12; i++)
890 {
891 if (z == znaky_zmien[i])
892 {
893 int8_t servo = zmeny[i];
894 pohyb(servo);
895 }
896 }
897 }
898
899 void kombinacia1()
900 {
901 pohyb(SERVO_LAVA_NOHA);
902 pohyb(-SERVO_PRAVA_PATA);
903 }
904
905 void kombinacia2()
906 {
907 pohyb(SERVO_PRAVA_NOHA);
908 pohyb(-SERVO_LAVA_PATA);
909 }
910
911 void kombinacia3()
912 {
913 pohyb(SERVO_LAVA_RUKA);
914 pohyb(SERVO_PRAVA_RUKA);
915 }
916
917 void kombinacia4()
918 {
919 pohyb(-SERVO_LAVA_RUKA);
920 pohyb(-SERVO_PRAVA_RUKA);
921 }
922
923 int pohyb_kombinacia(char z)
924 {
925 if (z == '3') kombinacia1();
926 else if (z == '4') kombinacia2();
927 else if (z == '5') kombinacia3();
928 else if (z == '6') kombinacia4();
929 else return 0;
930 return 1;
931 }
932
933 int nacitajCislo()
934 {
935 int num = 0;
936 int z;
937 do {
938 z = read_char();
939 if (z == '#') while (z != 13) z = read_char();
940 } while ((z < '0') || (z > '9'));
941 while ((z >= '0') && (z <= '9'))
942 {
943 num *= 10;
944 num += (z - '0');
945 do {
946 z = read_char();
947 if (z == -1) delayMicroseconds(10);
948 } while (z < 0);
949 }
950 return num;
951 }
952
953 void nacitaj_choreografiu()
954 {
955 ch_len = 0;
956 int tm;
957 do {
958 tm = nacitajCislo();
959 ch_time[ch_len] = tm;
960 ch_servo[ch_len] = nacitajCislo();
961 ch_val[ch_len] = nacitajCislo();
962 ch_len++;
963 if (ch_len == CHOREO_LEN) break;
964 } while (tm > 0);
965 pipni();
966 }
967
968 void vypis_choreografiu()
969 {
970 for (int i = 0; i < ch_len; i++)
971 {
972 serial_print_num(ch_time[i]);
973 serial_print(" ");
974 serial_print_num(ch_servo[i]);
975 serial_print(" ");
976 serial_println_num(ch_val[i]);
977 }
978 serial_println_flash(PSTR("---"));
979 pipni();
980 }
981
982 uint32_t koniec_tanca;
983
984 void zatancuj_choreografiu(uint16_t *ch_time, uint8_t *ch_servo, uint8_t *ch_val, int ch_len )
985 {
986 koniec_tanca = millis() + 3600000;
987 for (int i = 0; i < ch_len - 1; i++)
988 {
989 delay(ch_time[i]);
990 if (millis() > koniec_tanca) break;
991 if (ch_servo[i] < 7) nastav_koncatinu(ch_servo[i], ch_val[i]);
992 else i = specialny_prikaz(i, ch_servo[i], ch_val[i]);
993 if (serial_available() || Serial.available()) break;
994 }
995 pipni();
996 }
997
998 int specialny_prikaz(uint16_t i, uint8_t prikaz, uint8_t argument)
999 {
1000 if (prikaz == 8) koniec_tanca = millis() + 1000 * (uint32_t)argument;
1001 else if (prikaz == 9) return ((int)argument) - 1;
1002 else if (prikaz == 10) spomalenie = argument;
1003 else if (prikaz == 11) zahraj_melodiu(argument);
1004 else if (prikaz == 12) zvukovy_efekt(argument);
1005 return i;
1006 }
1007
1008 void reset()
1009 {
1010 for (int i = 0; i < 6; i++)
1011 {
1012 stav[i] = kalib[i];
1013 s[i].write(kalib[i]);
1014 delay(300);
1015 }
1016 pipni();
1017 }
1018
1019 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota, uint8_t min_hodnota, uint8_t max_hodnota)
1020 {
1021 serial_print_flash(PSTR(" (+/-/ENTER): "));
1022 serial_println_num(hodnota);
1023 s[servo].write(hodnota);
1024 char z;
1025 do {
1026 z = read_char();
1027 if ((z == '+') && (hodnota < max_hodnota)) hodnota++;
1028 else if ((z == '-') && (hodnota > min_hodnota)) hodnota--;
1029 if ((z == '+') || (z == '-'))
1030 {
1031 serial_print_num(hodnota); serial_print_char('\r');
1032 s[servo].write(hodnota);
1033 }
1034 } while (z != 13);
1035 return hodnota;
1036 }
1037
1038 void kalibruj()
1039 {
1040 for (int i = 0; i < 6; i++)
1041 {
1042 serial_print_num(i);
1043 kalib[i] = nalad_hodnotu_serva(i, kalib[i], 90-63, 90+63);
1044 serial_print_num(i);
1045 serial_print(": ");
1046 serial_println_num(kalib[i]);
1047 }
1048 for (int i = 0; i < 6; i++) {
1049 serial_print_num(kalib[i]);
1050 serial_print(" ");
1051 }
1052 serial_print_flash(PSTR(" Chyt robota zozadu za telo (ENTER):"));
1053 char z;
1054 do { z = read_char(); } while (z != 13);
1055 serial_println(z);
1056
1057 serial_print_flash(PSTR("*** Nasledujuce koncatiny su lave/prave z pohladu robota.\r\nAk to nesedi, treba prehodit kabliky servo!\r\n"));
1058
1059 for (int i = 0; i < 6; i++) servo_invertovane[i] = 0;
1060
1061 nastav_koncatinu(SERVO_PRAVA_PATA, 0);
1062 serial_print_flash(PSTR("Prava pata: je von alebo dnu? (V/D):"));
1063 do { z = read_char(); } while ((z != 'V') && (z != 'D'));
1064 serial_println_char(z);
1065 nastav_koncatinu(SERVO_PRAVA_PATA, 90);
1066 if (z == 'D') servo_invertovane[SERVO_PRAVA_PATA - 1] = 0;
1067 else servo_invertovane[SERVO_PRAVA_PATA - 1] = 1;
1068
1069 nastav_koncatinu(SERVO_LAVA_PATA, 0);
1070 serial_print_flash(PSTR("Lava pata: je von alebo dnu? (V/D):"));
1071 do { z = read_char(); } while ((z != 'V') && (z != 'D'));
1072 serial_println_char(z);
1073 nastav_koncatinu(SERVO_LAVA_PATA, 90);
1074 if (z == 'V') servo_invertovane[SERVO_LAVA_PATA - 1] = 0;
1075 else servo_invertovane[SERVO_LAVA_PATA - 1] = 1;
1076
1077 nastav_koncatinu(SERVO_PRAVA_NOHA, 0);
1078 serial_print_flash(PSTR("Prava noha: vpredu je pata alebo spicka? (P/S):"));
1079 do { z = read_char(); } while ((z != 'P') && (z != 'S'));
1080 serial_println_char(z);
1081 nastav_koncatinu(SERVO_PRAVA_NOHA, 90);
1082 if (z == 'P') servo_invertovane[SERVO_PRAVA_NOHA - 1] = 1;
1083 else servo_invertovane[SERVO_PRAVA_NOHA - 1] = 0;
1084
1085 nastav_koncatinu(SERVO_LAVA_NOHA, 0);
1086 serial_print_flash(PSTR("Lava noha: vpredu je pata alebo spicka? (P/S):"));
1087 do { z = read_char(); } while ((z != 'P') && (z != 'S'));
1088 serial_println_char(z);
1089 nastav_koncatinu(SERVO_LAVA_NOHA, 90);
1090 if (z == 'S') servo_invertovane[SERVO_LAVA_NOHA - 1] = 1;
1091 else servo_invertovane[SERVO_LAVA_NOHA - 1] = 0;
1092
1093 nastav_koncatinu(SERVO_PRAVA_RUKA, 0);
1094 serial_print_flash(PSTR("Prava ruka: je hore alebo dole? (H/D):"));
1095 do { z = read_char(); } while ((z != 'H') && (z != 'D'));
1096 serial_println_char(z);
1097 nastav_koncatinu(SERVO_PRAVA_RUKA, 90);
1098 if (z == 'H') servo_invertovane[SERVO_PRAVA_RUKA - 1] = 1;
1099 else servo_invertovane[SERVO_PRAVA_RUKA - 1] = 0;
1100
1101 nastav_koncatinu(SERVO_LAVA_RUKA, 0);
1102 serial_print_flash(PSTR("Lava ruka: je hore alebo dole? (H/D):"));
1103 do { z = read_char(); } while ((z != 'H') && (z != 'D'));
1104 serial_println_char(z);
1105 nastav_koncatinu(SERVO_LAVA_RUKA, 90);
1106 if (z == 'D') servo_invertovane[SERVO_PRAVA_RUKA - 1] = 0;
1107 else servo_invertovane[SERVO_PRAVA_RUKA - 1] = 1;
1108
1109 serial_println_flash(PSTR("ok"));
1110 pipni();
1111 }
1112
1113 void nastav_limity()
1114 {
1115 for (int i = 0; i < 6; i++)
1116 {
1117 serial_print_num(i);
1118 serial_print_flash(PSTR("dolny"));
1119 dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i], 0, 180);
1120 serial_print_num(i);
1121 serial_print_flash(PSTR(" dolny: "));
1122 serial_println_num(dolny_limit[i]);
1123 s[i].write(kalib[i]);
1124
1125 serial_print_num(i);
1126 serial_print_flash(PSTR("horny"));
1127 horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i], 0, 180);
1128 serial_print_num(i);
1129 serial_print_flash(PSTR(" horny: "));
1130 serial_println_num(horny_limit[i]);
1131 s[i].write(kalib[i]);
1132 }
1133 for (int i = 0; i < 6; i++) {
1134 serial_print_num(dolny_limit[i]);
1135 serial_print("-");
1136 serial_print_num(horny_limit[i]);
1137 serial_print(" ");
1138 }
1139 serial_println_flash(PSTR("ok"));
1140 pipni();
1141 }
1142
1143 void vypis_kalibraciu()
1144 {
1145 serial_print_flash(PSTR("stredy: "));
1146 for (int i = 0; i < 6; i++) {
1147 serial_print_num(kalib[i]);
1148 serial_print(" ");
1149 }
1150 serial_println();
1151
1152 serial_print_flash(PSTR("invertovane servo: "));
1153 for (int i = 0; i < 6; i++)
1154 {
1155 serial_print_num(servo_invertovane[i]);
1156 serial_print_char(' ');
1157 }
1158 serial_println();
1159
1160 serial_print_flash(PSTR("dolny limit: "));
1161 for (int i = 0; i < 6; i++) {
1162 serial_print_num(dolny_limit[i]);
1163 serial_print(" ");
1164 }
1165 serial_println();
1166 serial_print_flash(PSTR("horny limit: "));
1167 for (int i = 0; i < 6; i++) {
1168 serial_print_num(horny_limit[i]);
1169 serial_print(" ");
1170 }
1171 serial_println();
1172 }
1173
1174 void nacitaj_kalibraciu()
1175 {
1176 int tm;
1177 for (int i = 0; i < 6; i++)
1178 kalib[i] = nacitajCislo();
1179 vypis_kalibraciu();
1180 serial_println_flash(PSTR("ok"));
1181 pipni();
1182 }
1183
1184 void zvys_krok()
1185 {
1186 if (krok < 180) krok++;
1187 serial_print_flash(PSTR("krok: "));
1188 serial_println_num(krok);
1189 }
1190
1191 void zniz_krok()
1192 {
1193 if (krok > 0) krok--;
1194 serial_print_flash(PSTR("krok: "));
1195 serial_println_num(krok);
1196 }
1197
1198 void zniz_spomalenie()
1199 {
1200 if (spomalenie > 0) spomalenie--;
1201 serial_print_flash(PSTR("spomalenie: "));
1202 serial_println_num(spomalenie);
1203 }
1204
1205 void zvys_spomalenie()
1206 {
1207 if (spomalenie < 100) spomalenie++;
1208 serial_print_flash(PSTR("spomalenie: "));
1209 serial_println_num(spomalenie);
1210 }
1211
1212 // nasleduje softverova implementacia serioveho portu
1213 #define SERIAL_STATE_IDLE 0
1214 #define SERIAL_STATE_RECEIVING 1
1215 #define SERIAL_BUFFER_LENGTH 20
1216
1217 static volatile uint8_t serial_state;
1218 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
1219 static volatile uint8_t serial_buf_wp, serial_buf_rp;
1220
1221 static volatile uint8_t receiving_byte;
1222
1223 static volatile uint32_t time_startbit_noticed;
1224 static volatile uint8_t next_bit_order;
1225 static volatile uint8_t waiting_stop_bit;
1226 static uint16_t one_byte_duration;
1227 static uint16_t one_bit_duration;
1228 static uint16_t one_bit_write_duration;
1229 static uint16_t half_of_one_bit_duration;
1230
1231 void init_serial(uint32_t baud_rate)
1232 {
1233 pinMode(2, INPUT);
1234 pinMode(4, OUTPUT);
1235
1236 serial_state = SERIAL_STATE_IDLE;
1237
1238 one_byte_duration = 9500000 / baud_rate;
1239 one_bit_duration = 1000000 / baud_rate;
1240 one_bit_write_duration = one_bit_duration - 1;
1241 half_of_one_bit_duration = 500000 / baud_rate;
1242
1243 PCMSK2 |= 4; //PCINT18;
1244 PCIFR &= ~4; //PCIF2;
1245 PCICR |= 4; // PCIE2;
1246 }
1247
1248 ISR(PCINT2_vect)
1249 {
1250 uint32_t tm = micros();
1251 if (serial_state == SERIAL_STATE_IDLE)
1252 {
1253 time_startbit_noticed = tm;
1254 serial_state = SERIAL_STATE_RECEIVING;
1255 receiving_byte = 0xFF;
1256 next_bit_order = 0;
1257 }
1258 else if (tm - time_startbit_noticed > one_byte_duration)
1259 {
1260 serial_buffer[serial_buf_wp] = receiving_byte;
1261 serial_buf_wp++;
1262 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1263 time_startbit_noticed = tm;
1264 receiving_byte = 0xFF;
1265 next_bit_order = 0;
1266 }
1267 else if (PIND & 4)
1268 {
1269 int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
1270 while (next_bit_order < new_next_bit_order)
1271 {
1272 receiving_byte &= ~(1 << next_bit_order);
1273 next_bit_order++;
1274 }
1275 if (next_bit_order == 8)
1276 {
1277 serial_buffer[serial_buf_wp] = receiving_byte;
1278 serial_buf_wp++;
1279 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1280 serial_state = SERIAL_STATE_IDLE;
1281 }
1282 } else
1283 next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
1284 }
1285
1286 uint8_t serial_available()
1287 {
1288 cli();
1289 if (serial_buf_rp != serial_buf_wp)
1290 {
1291 sei();
1292 return 1;
1293 }
1294 if (serial_state == SERIAL_STATE_RECEIVING)
1295 {
1296 uint32_t tm = micros();
1297 if (tm - time_startbit_noticed > one_byte_duration)
1298 {
1299 serial_state = SERIAL_STATE_IDLE;
1300 serial_buffer[serial_buf_wp] = receiving_byte;
1301 serial_buf_wp++;
1302 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1303 sei();
1304 return 1;
1305 }
1306 }
1307 sei();
1308 return 0;
1309 }
1310
1311 int16_t serial_read()
1312 {
1313 cli();
1314 if (serial_buf_rp != serial_buf_wp)
1315 {
1316 uint8_t ch = serial_buffer[serial_buf_rp];
1317 serial_buf_rp++;
1318 if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
1319 sei();
1320 return ch;
1321 }
1322
1323 if (serial_state == SERIAL_STATE_RECEIVING)
1324 {
1325 uint32_t tm = micros();
1326 if (tm - time_startbit_noticed > one_byte_duration)
1327 {
1328 uint8_t ch = receiving_byte;
1329 serial_state = SERIAL_STATE_IDLE;
1330 sei();
1331 return ch;
1332 }
1333 }
1334 sei();
1335 return -1;
1336 }
1337
1338 void serial_write(uint8_t ch)
1339 {
1340 #ifdef ECHO_BT_TO_USB
1341 Serial.print((char)ch);
1342 #endif
1343 PORTD &= ~16;
1344 delayMicroseconds(one_bit_write_duration);
1345 for (uint8_t i = 0; i < 8; i++)
1346 {
1347 if (ch & 1) PORTD |= 16;
1348 else PORTD &= ~16;
1349 ch >>= 1;
1350 delayMicroseconds(one_bit_write_duration);
1351 }
1352 PORTD |= 16;
1353 delayMicroseconds(one_bit_write_duration);
1354 delayMicroseconds(one_bit_write_duration);
1355 delayMicroseconds(one_bit_write_duration);
1356 delayMicroseconds(one_bit_write_duration);
1357 delayMicroseconds(one_bit_write_duration);
1358 }
1359
1360 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
1361 {
1362 uint16_t len;
1363 uint16_t ch;
1364 do {
1365 ch = serial_read();
1366 if (ch == 13) continue;
1367 } while (ch == -1);
1368
1369 do {
1370 if ((ch != 13) && (ch != 10) && (ch != -1))
1371 {
1372 *(ln++) = ch;
1373 max_length--;
1374 len++;
1375 }
1376 ch = serial_read();
1377 } while ((ch != 13) && max_length);
1378 *ln = 0;
1379 return len;
1380 }
1381
1382 void serial_print_num(int32_t number)
1383 {
1384 if (number < 0)
1385 {
1386 serial_write('-');
1387 number = -number;
1388 }
1389 int32_t rad = 1;
1390 while (number / rad) rad *= 10;
1391 if (number > 0) rad /= 10;
1392 while (rad)
1393 {
1394 serial_write((char)('0' + (number / rad)));
1395 number -= (number / rad) * rad;
1396 rad /= 10;
1397 }
1398 }
1399
1400 void serial_print_char(char ch)
1401 {
1402 serial_write(ch);
1403 }
1404
1405 void serial_print(const uint8_t *str)
1406 {
1407 while (*str) serial_write(*(str++));
1408 }
1409
1410 void serial_println(const uint8_t *str)
1411 {
1412 serial_print(str);
1413 serial_write(13);
1414 serial_write(10);
1415 }
1416
1417 void serial_print_flash(const uint8_t PROGMEM *str)
1418 {
1419 int ln = strlen_P(str);
1420 for (int i = 0; i < ln; i++)
1421 serial_write(pgm_read_byte(str + i));
1422 }
1423
1424 void serial_println_flash(const uint8_t PROGMEM *str)
1425 {
1426 serial_print_flash(str);
1427 serial_write(13);
1428 serial_write(10);
1429 }
1430
1431 void serial_println_num(int32_t number)
1432 {
1433 serial_print_num(number);
1434 serial_println();
1435 }
1436
1437 void serial_println_char(char ch)
1438 {
1439 serial_write(ch);
1440 serial_println();
1441 }
1442
1443 void serial_println()
1444 {
1445 serial_write(13);
1446 serial_write(10);
1447 }
1448
1449 // nasleduje citanie z utltazvukoveho senzora
1450
1451 static volatile uint32_t pulse_start;
1452 static volatile uint8_t new_distance;
1453
1454 void init_ultrasonic()
1455 {
1456 pinMode(US_ECHO, INPUT);
1457 pinMode(US_TRIG, OUTPUT);
1458
1459 PCMSK0 |= 1; //PCINT0;
1460 PCIFR &= ~1; //PCIF0;
1461 PCICR |= 1; // PCIE0;
1462 }
1463
1464 ISR(PCINT0_vect)
1465 {
1466 if (PINB & 1) pulse_start = micros();
1467 else
1468 {
1469 distance = (int16_t)((micros() - pulse_start) / 58);
1470 new_distance = 1;
1471 }
1472 }
1473
1474 void start_distance_measurement()
1475 {
1476 distance = 10000;
1477 new_distance = 0;
1478 digitalWrite(US_TRIG, HIGH);
1479 delayMicroseconds(10);
1480 digitalWrite(US_TRIG, LOW);
1481 }
1482
1483 void wait_for_distance_measurement_to_complete()
1484 {
1485 uint8_t counter = 0;
1486 while ((counter < 20) && !new_distance)
1487 {
1488 delay(1);
1489 counter++;
1490 }
1491 if (counter == 20)
1492 {
1493 pinMode(US_ECHO, OUTPUT);
1494 digitalWrite(US_ECHO, HIGH);
1495 delayMicroseconds(10);
1496 digitalWrite(US_ECHO, LOW);
1497 pinMode(US_ECHO, INPUT);
1498 delayMicroseconds(5);
1499 distance = 10000;
1500 }
1501 }
1502
1503 int16_t measure_distance()
1504 {
1505 start_distance_measurement();
1506 wait_for_distance_measurement_to_complete();
1507 return distance;
1508 }
1509
1510 //-------------------------------- nasleduje prehravanie melodie a hranie cez timer2 v pozadi
1511 #define SIRENE_PORT PORTB
1512 #define SIRENE_DDR DDRB
1513 #define SIRENE_PIN 4
1514
1515 #define FIS3 2960
1516 #define G3 3136
1517
1518 float octave_4[] = { 2093.00, 2217.46, 2349.32, 2489.02, 2637.02, 2793.83, 2959.96, 3135.96, 3322.44, 3520.00, 3729.31, 3951.07 };
1519
1520 //popcorn
1521 uint16_t dlzka_melodia[] = {0, 386, 26, 281, 217};
1522 const uint8_t melodia1[] PROGMEM = { 252, 50, 149, 49,
1523 28, 31, 35, 40, 49, 99, 38, 49, 99, 40, 49, 99, 35, 49, 99, 31, 49, 99, 35, 49, 99, 28, 49, 99, 49,
1524 28, 31, 35, 40, 49, 99, 38, 49, 99, 40, 49, 99, 35, 49, 99, 31, 49, 99, 35, 49, 99, 28, 49, 99, 149,
1525 40, 49, 99, 42, 49, 99, 43, 49, 99, 42, 49, 99, 43, 49, 99, 40, 49, 99, 42, 49, 99, 40, 49, 99, 42, 49, 99, 38, 49, 99, 40, 49, 99, 38, 49, 99, 40, 49, 99, 36, 49, 99, 40, 49, 99,
1526 28, 31, 35, 40, 49, 99, 38, 49, 99, 40, 49, 99, 35, 49, 99, 31, 49, 99, 35, 49, 99, 28, 49, 99, 49,
1527 28, 31, 35, 40, 49, 99, 38, 49, 99, 40, 49, 99, 35, 49, 99, 31, 49, 99, 35, 49, 99, 28, 49, 99, 149,
1528 40, 49, 99, 42, 49, 99, 43, 49, 99, 42, 49, 99, 43, 49, 99, 40, 49, 99, 42, 49, 99, 40, 49, 99, 42, 49, 99, 38, 49, 99, 40, 49, 99, 38, 49, 99, 40, 49, 99, 42, 49, 99, 43, 49, 99,
1529 49, 35, 38, 43, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 38, 49, 99, 43, 49, 99, 35, 49, 99,
1530 49, 35, 38, 43, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 38, 49, 99, 43, 49, 99, 35, 49, 99, 149 ,
1531 47, 49, 99, 254, 49, 99, 255, 49, 99, 254, 49, 99, 255, 49, 99, 47, 49, 99, 254, 49, 99, 47, 49, 99, 254, 49, 99, 45, 49, 99, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 47, 49, 99,
1532 49, 35, 38, 43, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 38, 49, 99, 43, 49, 99, 35, 49, 99,
1533 49, 35, 38, 43, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 38, 49, 99, 43, 49, 99, 35, 49, 99, 149 ,
1534 47, 49, 99, 254, 49, 99, 255, 49, 99, 254, 49, 99, 255, 49, 99, 47, 49, 99, 254, 49, 99, 47, 49, 99, 254, 49, 99, 45, 49, 99, 47, 49, 99, 45, 49, 99, 47, 49, 99, 254, 49, 99, 255, 49, 99
1535 };
1536
1537 //kohutik jarabi
1538 const uint8_t melodia2[] PROGMEM = { 252, 150, 119, 121, 173, 174, 124, 124, 124, 123, 171, 173, 123, 123, 123, 121, 169, 171, 121, 121, 121, 123, 171, 169, 119, 119 };
1539
1540 // kankán
1541 const uint8_t melodia3[] PROGMEM = { 252, 100,
1542 251, 1, 184, 1, 32, 126, 149, 251, 1, 184, 1, 32, 126, 149, 251, 1, 184, 1, 32, 126, 251, 1, 184, 1, 32, 126, 251, 1, 184, 1, 32, 126, 251, 1, 184, 1, 32, 126,
1543 64, 71, 71, 73, 71, 69, 69, 73, 74, 78, 81, 78, 78, 76, 251, 1, 184, 1, 32, 126, 78, 68, 68, 78, 76, 69, 69, 73, 73, 71, 73, 71, 85, 83, 85, 83,
1544 64, 71, 71, 73, 71, 69, 69, 73, 74, 78, 81, 78, 78, 76, 251, 1, 184, 1, 32, 126, 78, 68, 68, 78, 76, 69, 69, 73, 73, 71, 73, 71, 71, 69, 119,
1545 135, 131, 128, 126, 75, 76, 78, 80, 81, 76, 80, 76, 81, 76, 80, 76, 81, 76, 80, 76, 81, 76, 80, 76,
1546 251, 2, 11, 3, 16, 19, 251, 1, 4, 3, 16, 19,
1547 251, 1, 4, 3, 16, 19, 251, 1, 4, 3, 16, 19,
1548 251, 1, 4, 3, 16, 19, 251, 1, 4, 3, 16, 19,
1549 251, 1, 4, 3, 16, 19, 251, 1, 4, 3, 16, 19,
1550 174, 88, 91, 90, 88, 143, 143, 93, 95, 90, 91, 138, 138, 88, 91, 90, 88, 86, 86, 85, 83, 81, 79, 78, 76,
1551 174, 88, 91, 90, 88, 143, 143, 93, 95, 90, 91, 138, 138, 88, 91, 90, 88, 86, 93, 89, 90, 136,
1552 64, 71, 71, 73, 71, 69, 69, 73, 74, 78, 81, 78, 78, 76, 251, 1, 184, 1, 32, 126,
1553 78, 76, 126, 78, 76, 126, 78, 76, 126, 78, 76, 126, 78, 76, 126, 78, 76, 126,
1554 78, 76, 78, 76, 78, 76, 78, 76,
1555 131, 119, 119, 119, 169
1556 };
1557
1558 //labutie jazero
1559 const uint8_t melodia4[] PROGMEM = {
1560 252, 220, 66, 69, 73, 69, 66, 69, 73, 69, 66, 69, 73, 69, 66, 69, 73, 69,
1561 185, 78, 80, 81, 83, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 78, 81, 78, 73, 81, 178,
1562 99, 83, 81, 80,
1563 185, 78, 80, 81, 83, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 78, 81, 78, 73, 81, 178,
1564 149, 128, 130, 131, 133, 85, 86, 251, 6, 32, 3, 8, 86, 135, 86, 88, 251, 6, 224, 3, 8, 88, 136, 88, 90, 251, 7, 184, 3, 8, 90, 85, 81, 80, 78,
1565 130, 131, 133, 85, 86, 251, 6, 32, 3, 8, 86, 135, 86, 88, 251, 6, 224, 3, 8, 88, 136, 88, 90, 251, 7, 73, 3, 8, 86, 133, 86, 91, 251, 7, 184, 3, 8, 87, 251, 7, 184, 3, 8, 85,
1566 185, 78, 80, 81, 83, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 78, 81, 78, 73, 81, 178,
1567 99, 83, 81, 80,
1568 185, 78, 80, 81, 83, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 78, 81, 78, 73, 81, 178
1569 };
1570
1571 volatile int16_t music_speed = 800 / 16;
1572
1573 volatile PGM_P current_note ;
1574 volatile uint16_t notes_remaining;
1575
1576 void zahraj_melodiu(uint8_t cislo)
1577 {
1578 if (cislo == 0) {
1579 zastav_melodiu();
1580 return;
1581 }
1582
1583 if (cislo == 1) current_note = melodia1;
1584 else if (cislo == 2) current_note = melodia2;
1585 else if (cislo == 3) current_note = melodia3;
1586 else if (cislo == 4) current_note = melodia4;
1587 notes_remaining = dlzka_melodia[cislo];
1588
1589 next_note();
1590 }
1591
1592 void next_note()
1593 {
1594 uint16_t freq, dur;
1595 if (!notes_remaining) return;
1596 otto_translate_tone_flash(&freq, &dur);
1597 tone2(freq, dur);
1598 }
1599
1600 void otto_translate_tone_flash(uint16_t *freq, uint16_t *del)
1601 {
1602 do {
1603 uint8_t n = pgm_read_byte(current_note);
1604 if (n == 251)
1605 {
1606 current_note++;
1607 uint8_t f1 = pgm_read_byte(current_note);
1608 current_note++;
1609 uint8_t f2 = pgm_read_byte(current_note);
1610 current_note++;
1611 uint8_t d1 = pgm_read_byte(current_note);
1612 current_note++;
1613 uint8_t d2 = pgm_read_byte(current_note);
1614 *freq = (f1 << 8) + f2;
1615 *del = (music_speed * 16 * (long)d1) / d2;
1616 notes_remaining -= 4;
1617 }
1618 else if (n == 252)
1619 {
1620 current_note++;
1621 music_speed = pgm_read_byte(current_note);
1622 current_note++;
1623 notes_remaining -= 2;
1624 continue;
1625 }
1626 else if (n == 254)
1627 {
1628 *freq = FIS3;
1629 *del = music_speed;
1630 }
1631 else if (n == 255)
1632 {
1633 *freq = G3;
1634 *del = music_speed;
1635 }
1636 else
1637 {
1638 uint8_t len = n / 50;
1639 *del = music_speed;
1640 while (len--) *del *= 2;
1641 n = n % 50;
1642 if (n != 49)
1643 {
1644 uint8_t octave = (n + 5) / 12;
1645 n = (n + 5) % 12;
1646 float ffreq = octave_4[n];
1647 octave = 4 - octave;
1648 while (octave > 0)
1649 {
1650 ffreq /= 2.0;
1651 octave--;
1652 }
1653 *freq = (uint16_t)ffreq;
1654 }
1655 else *freq = 0;
1656 }
1657 notes_remaining--;
1658 current_note++;
1659 break;
1660 } while (1);
1661 }
1662
1663 static volatile uint8_t tone2_state;
1664 static volatile uint8_t tone2_pause;
1665 static volatile uint32_t tone2_len;
1666
1667 void init_tone2()
1668 {
1669 notes_remaining = 0;
1670 tone2_pause = 0;
1671 TCCR2A = 2;
1672 TCCR2B = 0;
1673 TIMSK2 = 2;
1674 SIRENE_DDR |= (1 << SIRENE_PIN);
1675 }
1676
1677
1678 ISR(TIMER2_COMPA_vect)
1679 {
1680 if (!tone2_pause)
1681 {
1682 if (tone2_state)
1683 {
1684 SIRENE_PORT |= (1 << SIRENE_PIN);
1685 tone2_state = 0;
1686 }
1687 else
1688 {
1689 SIRENE_PORT &= ~(1 << SIRENE_PIN);
1690 tone2_state = 1;
1691 }
1692 }
1693 if ((--tone2_len) == 0)
1694 {
1695 TCCR2B = 0;
1696 tone2_pause = 0;
1697 next_note();
1698 }
1699 }
1700
1701 void tone2(uint16_t freq, uint16_t duration)
1702 {
1703 uint32_t period = ((uint32_t)1000000) / (uint32_t)freq;
1704
1705 if (freq >= 977) // prescaler 32
1706 {
1707 tone2_state = 0;
1708 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1709 if (tone2_len == 0) tone2_len++;
1710 TCNT2 = 0;
1711 OCR2A = (uint8_t) (250000 / (uint32_t)freq);
1712 TCCR2B = 3;
1713 }
1714 else if (freq >= 488) // prescaler 64
1715 {
1716 tone2_state = 0;
1717 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1718 if (tone2_len == 0) tone2_len++;
1719 TCNT2 = 0;
1720 OCR2A = (uint8_t) (125000 / (uint32_t)freq);
1721 TCCR2B = 4;
1722 }
1723 else if (freq >= 244) // prescaler 128
1724 {
1725 tone2_state = 0;
1726 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1727 if (tone2_len == 0) tone2_len++;
1728 TCNT2 = 0;
1729 OCR2A = (uint8_t) (62500 / (uint32_t)freq);
1730 TCCR2B = 5;
1731 }
1732 else if (freq >= 122) //prescaler 256
1733 {
1734 tone2_state = 0;
1735 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1736 if (tone2_len == 0) tone2_len++;
1737 TCNT2 = 0;
1738 OCR2A = (uint8_t) (31250 / (uint32_t)freq);
1739 TCCR2B = 6;
1740 }
1741 else if (freq >= 30) //prescaler 1024
1742 {
1743 tone2_state = 0;
1744 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1745 if (tone2_len == 0) tone2_len++;
1746 TCNT2 = 0;
1747 OCR2A = (uint8_t) (7813 / (uint32_t)freq);
1748 TCCR2B = 7;
1749 }
1750 else if (freq == 0)
1751 {
1752 tone2_pause = 1;
1753 tone2_state = 0;
1754 period = 1000000 / 500;
1755 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1756 TCNT2 = 0;
1757 OCR2A = (uint8_t) (125000 / (uint32_t)500);
1758 TCCR2B = 4;
1759 }
1760 else
1761 {
1762 TCCR2B = 0;
1763 }
1764 }
1765
1766 void zastav_melodiu()
1767 {
1768 notes_remaining = 0;
1769 }