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