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