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