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