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