Otto - riadiaci program v.3
Revision as of 22:11, 9 August 2018 by 95.105.230.221 (talk)
Program umožňuje priame riadenie robota (aj cez BlueTooth). Teraz už funguje aj na pinoch 2,4 a môže sa naraz programovať aj komunikovať.
Na komunikáciu s robotom môžete použiť napr. program Putty: putty.exe.
Pripojenie na vývody Arduina:
servo | pin | |
---|---|---|
ľavá ruka | 10 | |
pravá ruka | 11 | |
ľavá noha | 9 | |
pravá noha | 6 | |
ľavá päta | 5 | |
pravá päta | 3 | |
TXD BlueTooth | 2 | |
RXD BlueTooth | 4 | |
TRIG Ultrazvuk | 7 | |
ECHO Ultrazvuk | 8 | |
Sirena | 12 |
Číslovanie serva v choreografii:
servo | číslo serva v choreografii |
---|---|
ľavá ruka | 5 |
pravá ruka | 6 |
ľavá noha | 4 |
pravá noha | 3 |
ľavá päta | 2 |
pravá päta | 1 |
klaves | servo/funkcia |
---|---|
a/q | ľavá ruka |
;/p | pravá ruka |
z/x | ľavá noha |
,/. | pravá noha |
d/c | ľavá päta |
k/m | pravá päta |
1/9 | zníž/zvýš rýchlosť pohybu |
H | kalibrovať strednú polohu servomotorov
|
J | kalibrovať limity pre všetky stupne voľnosti (len pre priame riadenie) |
G | zobrazenie celej kalibrácie |
E | celú kalibráciu je možné zapísať do trvalej pamäte EEPROM (používať opatrne! - dá sa prepísať najviac 100000-krát), po zapnutí sa automaticky načíta |
minus | zvukový pozdrav |
R | ruky hore-dole |
@ | načítanie choreografie, pozri príklady a formát |
? | zobrazí načítanú choreografiu (kontrola po @) |
t | zatancuje načítanú choreografiu |
U | testuje ultrazvuk |
S | zobrazí aktuálnu polohu servomotorov (vhodné pri laden |
medzera | reset všetkých motorov do strednej polohy |
3,...,6 | zmiešaný pohyb (treba nastaviť vo funkciách kombinacia1() - kombinacia4() |
v,b,n,j,h,g,f,s,l,o,i,u,y,r,e,w | zvukové efekty 1-16 |
Pozrite si (a skopírujte si) príklady choreografií: Otto - príklady choreografií
Download: otto3.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 // cisla pinov, kde su zapojene servo motory
19 #define PIN_SERVO_LAVA_RUKA 10
20 #define PIN_SERVO_PRAVA_RUKA 11
21 #define PIN_SERVO_LAVA_NOHA 9
22 #define PIN_SERVO_PRAVA_NOHA 6
23 #define PIN_SERVO_LAVA_PATA 5
24 #define PIN_SERVO_PRAVA_PATA 3
25
26 #define S1 3
27 #define S2 5
28 #define S3 6
29 #define S4 9
30 #define S5 10
31 #define S6 11
32 #define SIRENA 12
33
34 //maximalna dlzka choreografie
35 #define CHOREO_LEN 200
36
37 // tu su serva cislovane 1-6
38 #define SERVO_LAVA_RUKA 5
39 #define SERVO_PRAVA_RUKA 6
40 #define SERVO_LAVA_NOHA 4
41 #define SERVO_PRAVA_NOHA 3
42 #define SERVO_LAVA_PATA 2
43 #define SERVO_PRAVA_PATA 1
44
45 // ak su niektore serva naopak, je tu jednotka
46 uint8_t servo_invertovane[6] = {0, 0, 1, 1, 0, 1};
47
48 // znaky, ktorymi sa ovladaju jednotlive stupne volnosti
49 char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' };
50 // co robia jednotlive znaky (znamienko urcuje smer)
51 int8_t zmeny[] = {SERVO_LAVA_RUKA, -SERVO_LAVA_RUKA,
52 SERVO_PRAVA_RUKA, -SERVO_PRAVA_RUKA,
53 -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA,
54 -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA,
55 SERVO_LAVA_PATA, -SERVO_LAVA_PATA,
56 SERVO_PRAVA_PATA, -SERVO_PRAVA_PATA };
57
58 // sem si mozno ulozit svoju kalibraciu
59 //uint8_t prednastavena_kalibracia[] = { 78, 69, 83, 80, 50, 67 };
60 uint8_t prednastavena_kalibracia[] = { 90, 90, 90, 90, 90, 90 };
61
62 uint8_t dolny_limit[] = {0, 0, 0, 0, 0, 0, 0, 0};
63 uint8_t horny_limit[] = {180, 180, 180, 180, 180, 180};
64
65 Servo s[6];
66
67 uint16_t ch_time[CHOREO_LEN];
68 uint8_t ch_servo[CHOREO_LEN];
69 uint8_t ch_val[CHOREO_LEN];
70 int ch_len;
71 uint8_t kalib[6];
72 int stav[6];
73 int krok;
74 uint8_t spomalenie;
75
76 static volatile int16_t distance;
77
78 #define FIS3 2960
79 #define G3 3136
80
81 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 };
82 int16_t music_speed;
83
84 //popcorn
85 uint16_t dlzka_melodia1 = 390;
86 const uint8_t melodia1[] PROGMEM = { 149, 49,
87 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,
88 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,
89 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,
90 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,
91 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,
92 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,
93 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,
94 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 ,
95 40, 49, 99, 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,
96 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,
97 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 ,
98 40, 49, 99, 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
99 };
100
101 void setup() {
102 Serial.begin(9600);
103 init_serial(9600);
104 init_ultrasonic();
105
106 randomSeed(analogRead(1));
107 s[0].attach(PIN_SERVO_PRAVA_PATA);
108 s[1].attach(PIN_SERVO_LAVA_PATA);
109 s[2].attach(PIN_SERVO_PRAVA_NOHA);
110 s[3].attach(PIN_SERVO_LAVA_NOHA);
111 s[4].attach(PIN_SERVO_LAVA_RUKA);
112 s[5].attach(PIN_SERVO_PRAVA_RUKA);
113 precitaj_kalibraciu_z_EEPROM();
114 for (int i = 0; i < 6; i++)
115 {
116 kalib[i] = prednastavena_kalibracia[i];
117 stav[i] = kalib[i];
118 s[i].write(stav[i]);
119 }
120 ch_len = 0;
121 krok = 7;
122 spomalenie = 6;
123 ahoj();
124 ruky2();
125 delay(100);
126 serial_println("\r\n Otto DTDT");
127 }
128
129 void loop() {
130 char z = -1;
131 if (serial_available()) z = serial_read();
132 #ifdef ECHO_BT_TO_USB
133 if (Serial.available()) z = Serial.read();
134 #endif
135
136 if (z != -1)
137 {
138 if (pohyb_znakom(z)) return;
139 else if (pohyb_kombinacia(z)) return;
140 else if (vydaj_zvukovy_efekt(z)) return;
141 else if (z == '@') nacitaj_choreografiu();
142 else if (z == '?') vypis_choreografiu();
143 else if (z == 't') zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
144 else if (z == '-') ahoj();
145 else if (z == ' ') reset();
146 else if (z == 'H') kalibruj();
147 else if (z == 'J') nastav_limity();
148 else if (z == 'G') vypis_kalibraciu();
149 else if (z == 'L') nacitaj_kalibraciu();
150 else if (z == 'E') zapis_kalibraciu_do_EEPROM();
151 else if (z == 'R') ruky();
152 else if (z == '9') zvys_krok();
153 else if (z == '1') zniz_krok();
154 else if (z == '8') zvys_spomalenie();
155 else if (z == '7') zniz_spomalenie();
156 else if (z == 'U') test_ultrazvuk();
157 else if (z == 'S') zobraz_stav();
158
159 }
160 int16_t d = measure_distance();
161 if (d < 10) menu_ultrasonic_request();
162 }
163
164 const uint8_t klavesy_efektov[] PROGMEM = {'v', 'b', 'n', 'j', 'h', 'g', 'f', 's', 'l', 'o', 'i', 'u', 'y', 'r', 'e', 'w'};
165 #define POCET_KLAVES_EFEKTOV 16
166
167 uint8_t vydaj_zvukovy_efekt(char z)
168 {
169 for (int i = 0; i < POCET_KLAVES_EFEKTOV; i++)
170 if (z == pgm_read_byte_near(klavesy_efektov + i)) zvukovy_efekt(i + 1);
171 }
172
173 void otto_translate_tone(uint8_t *note, uint16_t *freq, uint16_t *del)
174 {
175 if (*note == 251)
176 {
177 *freq = ((*(note + 1)) << 8) + (*(note + 2));
178 uint8_t d1 = *(note + 3);
179 uint8_t d2 = *(note + 4);
180 *del = (music_speed * 16 * (long)d1) / d2;
181 }
182 else if (*note == 254)
183 {
184 *freq = FIS3;
185 *del = music_speed;
186 }
187 else if (*note == 255)
188 {
189 *freq = G3;
190 *del = music_speed;
191 }
192 else
193 {
194 uint8_t len = *note / 50;
195 *del = music_speed;
196 while (len--) *del *= 2;
197 uint8_t n = *note % 50;
198 if (n != 49)
199 {
200 uint8_t octave = (n + 5) / 12;
201 n = (n + 5 ) % 12;
202 float ffreq = octave_4[n];
203 octave = 4 - octave;
204 while (octave > 0)
205 {
206 ffreq /= 2.0;
207 octave--;
208 }
209 *freq = (uint16_t)ffreq;
210 }
211 else *freq = 0;
212 }
213 }
214
215 void otto_tone(uint8_t *note)
216 {
217 uint16_t freq, d;
218 otto_translate_tone(note, &freq, &d);
219 if (freq) tone(SIRENA, freq); else noTone(SIRENA);
220 delay(d);
221 noTone(SIRENA);
222 }
223
224 void zahraj_melodiu1()
225 {
226 uint8_t nota[5];
227 music_speed = 800 / 16;
228 for (int j = 0; j < 4; j++)
229 {
230 for (int i = 0; i < dlzka_melodia1; i++)
231 {
232 nota[0] = pgm_read_byte_near(melodia1 + i);
233 if (nota[0] == 251)
234 {
235 for (int k = 1; k < 5; k++)
236 nota[k] = pgm_read_byte_near(melodia1 + i + k);
237 i += 4;
238 }
239 otto_tone(nota);
240 }
241 }
242 }
243
244 void efekt1()
245 {
246 uint16_t fre = 100;
247 for (int i = 0; i < 200; i++)
248 {
249 tone(SIRENA, fre);
250 fre += 10;
251 delay(2);
252 }
253 noTone(SIRENA);
254 }
255
256 void efekt2()
257 {
258 uint16_t fre = 2000;
259 for (int i = 0; i < 200; i++)
260 {
261 tone(SIRENA, fre);
262 fre -= 10;
263 delay(2);
264 }
265 noTone(SIRENA);
266 }
267
268 void efekt3()
269 {
270 uint16_t fre;
271 for (int i = 0; i < 200; i++)
272 {
273 fre = random(3000) + 20;
274 tone(SIRENA, fre);
275 delay(2);
276 }
277 noTone(SIRENA);
278 }
279
280 void efekt4()
281 {
282 uint16_t fre = 100;
283 for (int i = 0; i < 200; i++)
284 {
285 (i&1) ? tone(SIRENA, fre) : tone(SIRENA, 2100 - fre);
286 fre += 10;
287 delay(2);
288 }
289 noTone(SIRENA);
290 }
291
292 void efekt5()
293 {
294 uint16_t fre = 100;
295 for (int i = 0; i < 100; i++)
296 {
297 (i&1) ? tone(SIRENA, fre) : tone(SIRENA, 2100 - fre);
298 fre += 20;
299 delay(4);
300 }
301 noTone(SIRENA);
302 }
303
304 void efekt6()
305 {
306 uint16_t d = 12;
307 for (int i = 0; i < 20; i++)
308 {
309 tone(SIRENA, 1760, 10);
310 delay(d);
311 d += 3;
312 }
313 }
314
315 void efekt7()
316 {
317 for (int i = 0; i < 40; i++)
318 {
319 (i&1) ? noTone(SIRENA): tone(SIRENA, 1760);
320 delay(10);
321 }
322 noTone(SIRENA);
323 }
324
325 void efekt8()
326 {
327 uint16_t d = 72;
328 for (int i = 0; i < 20; i++)
329 {
330 tone(SIRENA, 1760, 10);
331 delay(d);
332 d -= 3;
333 }
334 }
335
336 void efekt9()
337 {
338 float fre = 3500;
339 while (fre > 15)
340 {
341 tone(SIRENA, (uint16_t)fre);
342 fre *= 0.97;
343 delay(2);
344 }
345 noTone(SIRENA);
346 }
347
348 void efekt10()
349 {
350 float fre = 55;
351 while (fre < 3000)
352 {
353 tone(SIRENA, (uint16_t)fre);
354 fre *= 1.03;
355 delay(2);
356 }
357 noTone(SIRENA);
358 }
359
360 void efekt11()
361 {
362 uint16_t fre = 3500;
363 uint16_t d = 42;
364 for (int i = 0; i < 20; i++)
365 {
366 tone(SIRENA, fre);
367 fre -= 165;
368 delay(d);
369 d -= 2;
370 }
371 noTone(SIRENA);
372 }
373
374 void efekt12()
375 {
376 float fre = 110;
377 uint8_t d = 42;
378 for (int i = 0; i < 20; i++)
379 {
380 tone(SIRENA, fre);
381 fre += 165;
382 delay(d);
383 d -= 2;
384 }
385 noTone(SIRENA);
386 }
387
388 void efekt13()
389 {
390 uint16_t fre = 3400;
391 uint8_t d = 200;
392 uint8_t delta = 1;
393 for (int i = 0; i < 20; i++)
394 {
395 tone(SIRENA, fre);
396 fre -= 165;
397 delay(d);
398 d -= delta;
399 delta++;
400 }
401 tone(SIRENA, 110);
402 delay(1000);
403 noTone(SIRENA);
404 }
405
406 void efekt14()
407 {
408 uint16_t fre = 880;
409 int16_t d = 20;
410 for (int i = 0; i < 20; i++)
411 {
412 tone(SIRENA, fre);
413 delay(d);
414 fre += random(50) - 25;
415 d += random(10) - 5;
416 if (d < 0) d = 1;
417 }
418 noTone(SIRENA);
419 }
420
421 void efekt15()
422 {
423 uint16_t fre = 440;
424 int16_t d = 20;
425 for (int i = 0; i < 20; i++)
426 {
427 tone(SIRENA, fre);
428 delay(d);
429 fre += random(25) - 12;
430 d += random(10) - 5;
431 if (d < 0) d = 1;
432 }
433 noTone(SIRENA);
434 }
435
436 void efekt16()
437 {
438 uint16_t fre = 1760;
439 int16_t d = 20;
440 for (int i = 0; i < 20; i++)
441 {
442 tone(SIRENA, fre);
443 delay(d);
444 fre += random(100) - 50;
445 d += random(10) - 5;
446 if (d < 0) d = 1;
447 }
448 noTone(SIRENA);
449 }
450
451 void zvukovy_efekt(uint8_t cislo)
452 {
453 switch (cislo) {
454 case 1: efekt1(); break;
455 case 2: efekt2(); break;
456 case 3: efekt3(); break;
457 case 4: efekt4(); break;
458 case 5: efekt5(); break;
459 case 6: efekt6(); break;
460 case 7: efekt7(); break;
461 case 8: efekt8(); break;
462 case 9: efekt9(); break;
463 case 10: efekt10(); break;
464 case 11: efekt11(); break;
465 case 12: efekt12(); break;
466 case 13: efekt13(); break;
467 case 14: efekt14(); break;
468 case 15: efekt15(); break;
469 case 16: efekt16(); break;
470 }
471 }
472
473 void test_ultrazvuk()
474 {
475 int i = 0;
476 while ((serial_available() == 0) && (Serial.available() == 0))
477 {
478 serial_println_num(measure_distance());
479 delay(100);
480 }
481 serial_read();
482 }
483
484 void menu_ultrasonic_request()
485 {
486 uint32_t tm = millis();
487 int d = measure_distance();
488 int count = 0;
489 int count2 = 0;
490 while ((millis() - tm < 1500) && ((d < 15) || (count2 < 5)) && (count < 10))
491 {
492 delay(10);
493 d = measure_distance();
494 if (d == 10000) count++;
495 else count = 0;
496 if (d >= 15) count2++;
497 else count2 = 0;
498 if (serial_available() || Serial.available()) return;
499 }
500 if (millis() - tm >= 1500)
501 ultrasonic_menu();
502 }
503
504 void ultrasonic_menu()
505 {
506 int selection = 0;
507 tone(SIRENA, 880, 200);
508
509 do {
510 int count = 0;
511 do {
512 int32_t d = measure_distance();
513 if (d == 10000) continue;
514 if (d >= 20) count++;
515 else count = 0;
516 delay(10);
517 } while (!serial_available() && !Serial.available() && (count < 20));
518
519 tone(SIRENA, 440, 200);
520 uint32_t tm = millis();
521 while ((measure_distance() > 15) && (millis() - tm < 1500) && !serial_available() && !Serial.available()) delay(10);
522 if (millis() - tm >= 1500)
523 {
524 tone(SIRENA, 2000, 50);
525 menu_command(selection);
526 return;
527 }
528 selection++;
529 for (int i = 0; i < selection; i++)
530 {
531 tone(SIRENA, 1261, 50);
532 delay(250);
533 }
534 } while (!serial_available() && !Serial.available());
535 while (serial_available()) serial_read();
536 while (Serial.available()) Serial.read();
537 }
538
539 void menu_command(int cmd)
540 {
541 if (cmd == 1) vpred();
542 if (cmd == 2) zahraj_melodiu1();
543 if (cmd == 3) melodia_jedna_druhej();
544 if (cmd == 4) ahoj();
545 serial_println_num(cmd);
546 }
547
548 void melodia_jedna_druhej()
549 {
550 for (int i = 0; i < 2; i++)
551 {
552 tone(SIRENA, 262, 200);
553 delay(200);
554 tone(SIRENA, 330, 200);
555 delay(200);
556
557 tone(SIRENA, 262, 200);
558 delay(200);
559 tone(SIRENA, 330, 200);
560 delay(200);
561
562 tone(SIRENA, 392, 400);
563 delay(400);
564
565 tone(SIRENA, 392, 400);
566 delay(400);
567 }
568 noTone(8);
569 }
570
571 // chodza pre vacsieho dreveneho robota
572 uint16_t chor_time[] = {100,1,1,100,1,1,1,100,1,1,1,100,1,100,1,0};
573 uint8_t chor_servo[] = {1,2,6,4,3,6,5,1,2,6,5,3,4,1,2,0};
574 uint8_t chor_val[] = {48,69,180,104,104,90,0,111,146,0,90,62,69,48,69,0};
575 uint8_t chor_len = 16;
576
577 void vpred()
578 {
579 while (measure_distance() > 30)
580 zatancuj_choreografiu(chor_time, chor_servo, chor_val, chor_len);
581 pipni();
582 reset();
583 }
584
585 void precitaj_kalibraciu_z_EEPROM()
586 {
587 uint8_t value = EEPROM.read(1);
588 if (value != '~') return;
589 for (int i = 2; i < 8; i++)
590 prednastavena_kalibracia[i - 2] = EEPROM.read(i);
591 for (int i = 0; i < 6; i++)
592 dolny_limit[i] = EEPROM.read(i + 9);
593 for (int i = 0; i < 6; i++)
594 horny_limit[i] = EEPROM.read(i + 15);
595 }
596
597 char read_char()
598 {
599 while (!serial_available() && !Serial.available());
600 if (serial_available()) return serial_read();
601 else return Serial.read();
602 }
603
604 void zapis_kalibraciu_do_EEPROM()
605 {
606 serial_print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
607 char odpoved = read_char();
608 serial_println_char(odpoved);
609 if (odpoved == 'Y')
610 {
611 EEPROM.write(1, '~');
612 for (int i = 2; i < 8; i++)
613 EEPROM.write(i, kalib[i - 2]);
614 for (int i = 0; i < 6; i++)
615 EEPROM.write(9 + i, dolny_limit[i]);
616 for (int i = 0; i < 6; i++)
617 EEPROM.write(15 + i, horny_limit[i]);
618 serial_println("ok");
619 }
620 }
621
622 void pipni()
623 {
624 tone(SIRENA, 1568, 50);
625 delay(100);
626 tone(SIRENA, 1357, 50);
627 }
628
629 void ruky()
630 {
631 int odloz_krok = krok;
632 delay(500);
633 krok = 90;
634 pohyb(SERVO_LAVA_RUKA);
635 pohyb(SERVO_PRAVA_RUKA);
636 delay(1000);
637 krok = 180;
638 pohyb(-SERVO_LAVA_RUKA);
639 pohyb(-SERVO_PRAVA_RUKA);
640 delay(1000);
641 krok = odloz_krok;
642 pipni();
643 }
644
645 void ruky2()
646 {
647 int odloz_krok = krok;
648 delay(500);
649 krok = 180;
650 pohyb(SERVO_LAVA_RUKA);
651 pohyb(SERVO_PRAVA_RUKA);
652 delay(1000);
653 krok = 90;
654 pohyb(-SERVO_LAVA_RUKA);
655 pohyb(-SERVO_PRAVA_RUKA);
656 delay(1000);
657 krok = odloz_krok;
658 pipni();
659 }
660
661 void ahoj()
662 {
663 tone(SIRENA, 1568, 50);
664 delay(70);
665 tone(SIRENA, 1175, 30);
666 delay(50);
667 tone(SIRENA, 880, 30);
668 delay(50);
669 tone(SIRENA, 1047, 50);
670 delay(70);
671 tone(SIRENA, 1245, 30);
672 delay(150);
673 tone(SIRENA, 1568, 50);
674 delay(100);
675 if (random(10) > 4) tone(SIRENA, 1357, 50);
676 else tone(SIRENA, 1047, 50);
677 }
678
679 void nastav_koncatinu(int8_t servo, uint8_t poloha)
680 {
681 int8_t delta;
682 int8_t srv = (servo > 0)?servo:-servo;
683 srv--;
684 poloha += kalib[srv] - 90;
685 if (poloha > 180) poloha = 180;
686 if (poloha < 0) poloha = 0;
687
688 if (stav[srv] < poloha) delta = 1;
689 else delta = -1;
690 while (stav[srv] != poloha)
691 {
692 stav[srv] +=delta;
693 s[srv].write(stav[srv]);
694 delay(spomalenie);
695 }
696 }
697
698 void zobraz_stav()
699 {
700 for (int i = 0; i < 6; i++)
701 {
702 serial_print("S"); serial_print_num(i + 1); serial_print(": "); serial_println_num(stav[i] - kalib[i] + 90);
703 }
704 serial_println("---");
705 pipni();
706 }
707
708 void pohyb(int8_t servo)
709 {
710 int8_t srv = (servo > 0)?servo:-servo;
711 srv--;
712 if (servo_invertovane[srv]) servo = -servo;
713 if (servo > 0)
714 {
715 if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
716 else stav[srv] = horny_limit[srv];
717 s[srv].write(stav[srv]);
718 }
719 else if (servo < 0)
720 {
721 if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok;
722 else stav[srv] = dolny_limit[srv];
723 s[srv].write(stav[srv]);
724 }
725 }
726
727 uint8_t pohyb_znakom(char z)
728 {
729 for (int i = 0; i < 12; i++)
730 {
731 if (z == znaky_zmien[i])
732 {
733 int8_t servo = zmeny[i];
734 pohyb(servo);
735 }
736 }
737 }
738
739 void kombinacia1()
740 {
741 pohyb(SERVO_LAVA_NOHA);
742 pohyb(-SERVO_PRAVA_PATA);
743 }
744
745 void kombinacia2()
746 {
747 pohyb(SERVO_PRAVA_NOHA);
748 pohyb(-SERVO_LAVA_PATA);
749 }
750
751 void kombinacia3()
752 {
753 pohyb(SERVO_LAVA_RUKA);
754 pohyb(SERVO_PRAVA_RUKA);
755 }
756
757 void kombinacia4()
758 {
759 pohyb(-SERVO_LAVA_RUKA);
760 pohyb(-SERVO_PRAVA_RUKA);
761 }
762
763 int pohyb_kombinacia(char z)
764 {
765 if (z == '3') kombinacia1();
766 else if (z == '4') kombinacia2();
767 else if (z == '5') kombinacia3();
768 else if (z == '6') kombinacia4();
769 else return 0;
770 return 1;
771 }
772
773 int nacitajCislo()
774 {
775 int num = 0;
776 int z;
777 do {
778 z = read_char();
779 if (z == '#') while (z != 13) z = read_char();
780 } while ((z < '0') || (z > '9'));
781 while ((z >= '0') && (z <= '9'))
782 {
783 num *= 10;
784 num += (z - '0');
785 do { z = read_char(); if (z == -1) delayMicroseconds(10); } while (z < 0);
786 }
787 return num;
788 }
789
790 void nacitaj_choreografiu()
791 {
792 ch_len = 0;
793 int tm;
794 do {
795 tm = nacitajCislo();
796 ch_time[ch_len] = tm;
797 ch_servo[ch_len] = nacitajCislo();
798 ch_val[ch_len] = nacitajCislo();
799 ch_len++;
800 if (ch_len == CHOREO_LEN) break;
801 } while (tm > 0);
802 pipni();
803 }
804
805 void vypis_choreografiu()
806 {
807 for (int i = 0; i < ch_len; i++)
808 {
809 serial_print_num(ch_time[i]);
810 serial_print(" ");
811 serial_print_num(ch_servo[i]);
812 serial_print(" ");
813 serial_println_num(ch_val[i]);
814 }
815 serial_println("---");
816 pipni();
817 }
818
819 void zatancuj_choreografiu(uint16_t *ch_time, uint8_t *ch_servo, uint8_t *ch_val, int ch_len )
820 {
821 for (int i = 0; i < ch_len - 1; i++)
822 {
823 delay(ch_time[i]);
824 if (ch_servo[i] < 10) nastav_koncatinu(ch_servo[i], ch_val[i]);
825 else specialny_prikaz(ch_servo[i], ch_val[i]);
826 }
827 pipni();
828 while (serial_available()) serial_read();
829 while (Serial.available()) Serial.read();
830 }
831
832 void specialny_prikaz(uint8_t prikaz, uint8_t argument)
833 {
834 if (prikaz == 10) spomalenie = argument;
835 else if (prikaz == 11) zacni_hrat_melodiu(argument);
836 else if (prikaz == 12) zvukovy_efekt(argument);
837 }
838
839 void zacni_hrat_melodiu(uint8_t cislo)
840 {
841
842 }
843
844 void reset()
845 {
846 for (int i = 0; i < 6; i++)
847 {
848 stav[i] = kalib[i];
849 s[i].write(kalib[i]);
850 delay(300);
851 }
852 pipni();
853 }
854
855 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
856 {
857 serial_print(" (+/-/ENTER): ");
858 serial_println_num(hodnota);
859 s[servo].write(hodnota);
860 char z;
861 do {
862 z = read_char();
863 if ((z == '+') && (hodnota < 180)) hodnota++;
864 else if ((z == '-') && (hodnota > 0)) hodnota--;
865 if ((z == '+') || (z == '-'))
866 {
867 serial_print_num(hodnota); serial_print_char('\r');
868 s[servo].write(hodnota);
869 }
870 } while (z != 13);
871 return hodnota;
872 }
873
874 void kalibruj()
875 {
876 for (int i = 0; i < 6; i++)
877 {
878 serial_print_num(i);
879 kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
880 serial_print_num(i);
881 serial_print(": ");
882 serial_println_num(kalib[i]);
883 }
884 for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
885 serial_println("ok");
886 pipni();
887 }
888
889 void nastav_limity()
890 {
891 for (int i = 0; i < 6; i++)
892 {
893 serial_print_num(i);
894 serial_print("dolny");
895 dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
896 serial_print_num(i);
897 serial_print(" dolny: ");
898 serial_println_num(dolny_limit[i]);
899 s[i].write(kalib[i]);
900
901 serial_print_num(i);
902 serial_print("horny");
903 horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
904 serial_print_num(i);
905 serial_print(" horny: ");
906 serial_println_num(horny_limit[i]);
907 s[i].write(kalib[i]);
908 }
909 for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print("-"); serial_print_num(horny_limit[i]); serial_print(" "); }
910 serial_println("ok");
911 pipni();
912 }
913
914 void vypis_kalibraciu()
915 {
916 serial_print("stredy: ");
917 for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
918 serial_println();
919 serial_print("dolny limit: ");
920 for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print(" "); }
921 serial_println();
922 serial_print("horny limit: ");
923 for (int i = 0; i < 6; i++) { serial_print_num(horny_limit[i]); serial_print(" "); }
924 serial_println();
925 }
926
927 void nacitaj_kalibraciu()
928 {
929 int tm;
930 for (int i = 0; i < 6; i++)
931 kalib[i] = nacitajCislo();
932 vypis_kalibraciu();
933 serial_println("ok");
934 pipni();
935 }
936
937 void zvys_krok()
938 {
939 if (krok < 180) krok++;
940 serial_print("krok: ");
941 serial_println_num(krok);
942 }
943
944 void zniz_krok()
945 {
946 if (krok > 0) krok--;
947 serial_print("krok: ");
948 serial_println_num(krok);
949 }
950
951 void zniz_spomalenie()
952 {
953 if (spomalenie > 0) spomalenie--;
954 serial_print("spomalenie: ");
955 serial_println_num(spomalenie);
956 }
957
958 void zvys_spomalenie()
959 {
960 if (spomalenie < 100) spomalenie++;
961 serial_print("spomalenie: ");
962 serial_println_num(spomalenie);
963 }
964
965 // nasleduje softverova implementacia serioveho portu
966 #define SERIAL_STATE_IDLE 0
967 #define SERIAL_STATE_RECEIVING 1
968 #define SERIAL_BUFFER_LENGTH 20
969
970 static volatile uint8_t serial_state;
971 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
972 static volatile uint8_t serial_buf_wp, serial_buf_rp;
973
974 static volatile uint8_t receiving_byte;
975
976 static volatile uint32_t time_startbit_noticed;
977 static volatile uint8_t next_bit_order;
978 static volatile uint8_t waiting_stop_bit;
979 static uint16_t one_byte_duration;
980 static uint16_t one_bit_duration;
981 static uint16_t one_bit_write_duration;
982 static uint16_t half_of_one_bit_duration;
983
984 void init_serial(uint32_t baud_rate)
985 {
986 pinMode(2, INPUT);
987 pinMode(4, OUTPUT);
988
989 serial_state = SERIAL_STATE_IDLE;
990
991 one_byte_duration = 9500000 / baud_rate;
992 one_bit_duration = 1000000 / baud_rate;
993 one_bit_write_duration = one_bit_duration - 1;
994 half_of_one_bit_duration = 500000 / baud_rate;
995
996 PCMSK2 |= 4; //PCINT18;
997 PCIFR &= ~4; //PCIF2;
998 PCICR |= 4; // PCIE2;
999 }
1000
1001 ISR(PCINT2_vect)
1002 {
1003 uint32_t tm = micros();
1004 if (serial_state == SERIAL_STATE_IDLE)
1005 {
1006 time_startbit_noticed = tm;
1007 serial_state = SERIAL_STATE_RECEIVING;
1008 receiving_byte = 0xFF;
1009 next_bit_order = 0;
1010 }
1011 else if (tm - time_startbit_noticed > one_byte_duration)
1012 {
1013 serial_buffer[serial_buf_wp] = receiving_byte;
1014 serial_buf_wp++;
1015 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1016 time_startbit_noticed = tm;
1017 receiving_byte = 0xFF;
1018 next_bit_order = 0;
1019 }
1020 else if (PIND & 4)
1021 {
1022 int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
1023 while (next_bit_order < new_next_bit_order)
1024 {
1025 receiving_byte &= ~(1 << next_bit_order);
1026 next_bit_order++;
1027 }
1028 if (next_bit_order == 8)
1029 {
1030 serial_buffer[serial_buf_wp] = receiving_byte;
1031 serial_buf_wp++;
1032 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1033 serial_state = SERIAL_STATE_IDLE;
1034 }
1035 } else
1036 next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
1037 }
1038
1039 uint8_t serial_available()
1040 {
1041 cli();
1042 if (serial_buf_rp != serial_buf_wp)
1043 {
1044 sei();
1045 return 1;
1046 }
1047 if (serial_state == SERIAL_STATE_RECEIVING)
1048 {
1049 uint32_t tm = micros();
1050 if (tm - time_startbit_noticed > one_byte_duration)
1051 {
1052 serial_state = SERIAL_STATE_IDLE;
1053 serial_buffer[serial_buf_wp] = receiving_byte;
1054 serial_buf_wp++;
1055 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1056 sei();
1057 return 1;
1058 }
1059 }
1060 sei();
1061 return 0;
1062 }
1063
1064 int16_t serial_read()
1065 {
1066 cli();
1067 if (serial_buf_rp != serial_buf_wp)
1068 {
1069 uint8_t ch = serial_buffer[serial_buf_rp];
1070 serial_buf_rp++;
1071 if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
1072 sei();
1073 return ch;
1074 }
1075
1076 if (serial_state == SERIAL_STATE_RECEIVING)
1077 {
1078 uint32_t tm = micros();
1079 if (tm - time_startbit_noticed > one_byte_duration)
1080 {
1081 uint8_t ch = receiving_byte;
1082 serial_state = SERIAL_STATE_IDLE;
1083 sei();
1084 return ch;
1085 }
1086 }
1087 sei();
1088 return -1;
1089 }
1090
1091 void serial_write(uint8_t ch)
1092 {
1093 #ifdef ECHO_BT_TO_USB
1094 Serial.print((char)ch);
1095 #endif
1096 PORTD &= ~16;
1097 delayMicroseconds(one_bit_write_duration);
1098 for (uint8_t i = 0; i < 8; i++)
1099 {
1100 if (ch & 1) PORTD |= 16;
1101 else PORTD &= ~16;
1102 ch >>= 1;
1103 delayMicroseconds(one_bit_write_duration);
1104 }
1105 PORTD |= 16;
1106 delayMicroseconds(one_bit_write_duration);
1107 delayMicroseconds(one_bit_write_duration);
1108 delayMicroseconds(one_bit_write_duration);
1109 delayMicroseconds(one_bit_write_duration);
1110 delayMicroseconds(one_bit_write_duration);
1111 }
1112
1113 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
1114 {
1115 uint16_t len;
1116 uint16_t ch;
1117 do {
1118 ch = serial_read();
1119 if (ch == 13) continue;
1120 } while (ch == -1);
1121
1122 do {
1123 if ((ch != 13) && (ch != 10) && (ch != -1))
1124 {
1125 *(ln++) = ch;
1126 max_length--;
1127 len++;
1128 }
1129 ch = serial_read();
1130 } while ((ch != 13) && max_length);
1131 *ln = 0;
1132 return len;
1133 }
1134
1135 void serial_print_num(int32_t number)
1136 {
1137 if (number < 0)
1138 {
1139 serial_write('-');
1140 number = -number;
1141 }
1142 int32_t rad = 1;
1143 while (number / rad) rad *= 10;
1144 if (number > 0) rad /= 10;
1145 while (rad)
1146 {
1147 serial_write((char)('0' + (number / rad)));
1148 number -= (number / rad) * rad;
1149 rad /= 10;
1150 }
1151 }
1152
1153 void serial_print_char(char ch)
1154 {
1155 serial_write(ch);
1156 }
1157
1158 void serial_print(const uint8_t *str)
1159 {
1160 while (*str) serial_write(*(str++));
1161 }
1162
1163 void serial_println(const uint8_t *str)
1164 {
1165 serial_print(str);
1166 serial_write(13);
1167 serial_write(10);
1168 }
1169
1170 void serial_println_num(int32_t number)
1171 {
1172 serial_print_num(number);
1173 serial_println();
1174 }
1175
1176 void serial_println_char(char ch)
1177 {
1178 serial_write(ch);
1179 serial_println();
1180 }
1181
1182 void serial_println()
1183 {
1184 serial_write(13);
1185 serial_write(10);
1186 }
1187
1188 // nasleduje citanie z utltazvukoveho senzora
1189
1190 static volatile uint32_t pulse_start;
1191 static volatile uint8_t new_distance;
1192
1193 void init_ultrasonic()
1194 {
1195 pinMode(US_ECHO, INPUT);
1196 pinMode(US_TRIG, OUTPUT);
1197
1198 PCMSK0 |= 1; //PCINT0;
1199 PCIFR &= ~1; //PCIF0;
1200 PCICR |= 1; // PCIE0;
1201 }
1202
1203 ISR(PCINT0_vect)
1204 {
1205 if (PINB & 1) pulse_start = micros();
1206 else
1207 {
1208 distance = (int16_t)((micros() - pulse_start) / 58);
1209 new_distance = 1;
1210 }
1211 }
1212
1213 void start_distance_measurement()
1214 {
1215 distance = 10000;
1216 new_distance = 0;
1217 digitalWrite(US_TRIG, HIGH);
1218 delayMicroseconds(10);
1219 digitalWrite(US_TRIG, LOW);
1220 }
1221
1222 void wait_for_distance_measurement_to_complete()
1223 {
1224 uint8_t counter = 0;
1225 while ((counter < 20) && !new_distance)
1226 {
1227 delay(1);
1228 counter++;
1229 }
1230 if (counter == 20)
1231 {
1232 pinMode(US_ECHO, OUTPUT);
1233 digitalWrite(US_ECHO, HIGH);
1234 delayMicroseconds(10);
1235 digitalWrite(US_ECHO, LOW);
1236 pinMode(US_ECHO, INPUT);
1237 delayMicroseconds(5);
1238 distance = 10000;
1239 }
1240 }
1241
1242 int16_t measure_distance()
1243 {
1244 start_distance_measurement();
1245 wait_for_distance_measurement_to_complete();
1246 return distance;
1247 }