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