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