1 #include <Servo.h>
2 #include <EEPROM.h>
3
4 #define ECHO_BT_TO_USB 1
5
6 #define US_TRIG 7
7 #define US_ECHO 8
8
9 #define BT_RX 2
10 #define BT_TX 4
11
12 #define SIRENA 12
13
14 //maximalna dlzka choreografie
15 #define CHOREO_LEN 200
16
17 // cisla pinov, kde su zapojene servo motory
18 #define PIN_SERVO_LAVA_RUKA 10
19 #define PIN_SERVO_PRAVA_RUKA 11
20 #define PIN_SERVO_LAVA_NOHA 9
21 #define PIN_SERVO_PRAVA_NOHA 6
22 #define PIN_SERVO_LAVA_PATA 5
23 #define PIN_SERVO_PRAVA_PATA 3
24
25 #define S1 3
26 #define S2 5
27 #define S3 6
28 #define S4 9
29 #define S5 10
30 #define S6 11
31 #define SIRENA 12
32
33 //maximalna dlzka choreografie
34 #define CHOREO_LEN 200
35
36 // tu su serva cislovane 1-6
37 #define SERVO_LAVA_RUKA 5
38 #define SERVO_PRAVA_RUKA 6
39 #define SERVO_LAVA_NOHA 4
40 #define SERVO_PRAVA_NOHA 3
41 #define SERVO_LAVA_PATA 2
42 #define SERVO_PRAVA_PATA 1
43
44 // ak su niektore serva naopak, je tu jednotka
45 uint8_t servo_invertovane[6] = {0, 0, 1, 1, 0, 1};
46
47 // znaky, ktorymi sa ovladaju jednotlive stupne volnosti
48 char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' };
49 // co robia jednotlive znaky (znamienko urcuje smer)
50 int8_t zmeny[] = {SERVO_LAVA_RUKA, -SERVO_LAVA_RUKA,
51 SERVO_PRAVA_RUKA, -SERVO_PRAVA_RUKA,
52 -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA,
53 -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA,
54 SERVO_LAVA_PATA, -SERVO_LAVA_PATA,
55 SERVO_PRAVA_PATA, -SERVO_PRAVA_PATA };
56
57 // sem si mozno ulozit svoju kalibraciu
58 //uint8_t prednastavena_kalibracia[] = { 78, 69, 83, 80, 50, 67 };
59 uint8_t prednastavena_kalibracia[] = { 90, 90, 90, 90, 90, 90 };
60
61 uint8_t dolny_limit[] = {0, 0, 0, 0, 0, 0, 0, 0};
62 uint8_t horny_limit[] = {180, 180, 180, 180, 180, 180};
63
64 Servo s[6];
65
66 uint16_t ch_time[CHOREO_LEN];
67 uint8_t ch_servo[CHOREO_LEN];
68 uint8_t ch_val[CHOREO_LEN];
69 int ch_len;
70 uint8_t kalib[6];
71 int stav[6];
72 int krok;
73 uint8_t spomalenie;
74
75 static volatile int16_t distance;
76
77 uint8_t *melodia;
78 uint16_t dlzka_melodie;
79
80
81 #define FIS3 2960
82 #define G3 3136
83
84 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 };
85 int16_t music_speed;
86
87 uint16_t dlzka_popcorn = 390;
88 uint8_t popcorn[] = { 149, 49,
89 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,
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, 149,
91 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,
92 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,
93 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,
94 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,
95 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,
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, 149 ,
97 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,
98 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,
99 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 ,
100 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
101 };
102
103
104 void setup() {
105 Serial.begin(9600);
106 init_serial(9600);
107 init_ultrasonic();
108
109 melodia = popcorn;
110 dlzka_melodie = dlzka_popcorn;
111
112 randomSeed(analogRead(1));
113 s[0].attach(PIN_SERVO_PRAVA_PATA);
114 s[1].attach(PIN_SERVO_LAVA_PATA);
115 s[2].attach(PIN_SERVO_PRAVA_NOHA);
116 s[3].attach(PIN_SERVO_LAVA_NOHA);
117 s[4].attach(PIN_SERVO_LAVA_RUKA);
118 s[5].attach(PIN_SERVO_PRAVA_RUKA);
119 precitaj_kalibraciu_z_EEPROM();
120 for (int i = 0; i < 6; i++)
121 {
122 kalib[i] = prednastavena_kalibracia[i];
123 stav[i] = kalib[i];
124 s[i].write(stav[i]);
125 }
126 ch_len = 0;
127 krok = 7;
128 spomalenie = 6;
129 ahoj();
130 ruky2();
131 delay(100);
132 serial_println("\r\n Otto DTDT");
133 }
134
135 void loop() {
136 char z = -1;
137 if (serial_available()) z = serial_read();
138 #ifdef ECHO_BT_TO_USB
139 if (Serial.available()) z = Serial.read();
140 #endif
141
142 if (z != -1)
143 {
144 if (pohyb_znakom(z)) return;
145 else if (pohyb_kombinacia(z)) return;
146 else if (z == '@') nacitaj_choreografiu();
147 else if (z == '?') vypis_choreografiu();
148 else if (z == 't') zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
149 else if (z == '-') ahoj();
150 else if (z == ' ') reset();
151 else if (z == 'H') kalibruj();
152 else if (z == 'J') nastav_limity();
153 else if (z == 'G') vypis_kalibraciu();
154 else if (z == 'L') nacitaj_kalibraciu();
155 else if (z == 'E') zapis_kalibraciu_do_EEPROM();
156 else if (z == 'R') ruky();
157 else if (z == '9') zvys_krok();
158 else if (z == '1') zniz_krok();
159 else if (z == '8') zvys_spomalenie();
160 else if (z == '7') zniz_spomalenie();
161 else if (z == 'U') test_ultrazvuk();
162 else if (z == 'S') zobraz_stav();
163 }
164 int16_t d = measure_distance();
165 if (d < 10) menu_ultrasonic_request();
166 }
167
168
169
170 void otto_tone(uint8_t *note)
171 {
172 if (*note == 251)
173 {
174 uint16_t frq = ((*(note + 1)) << 8) + (*(note + 2));
175 uint8_t d1 = *(note + 3);
176 uint8_t d2 = *(note + 4);
177 uint16_t del = (music_speed * 16 * (long)d1) / d2;
178
179 tone(SIRENA, frq);
180 delay(del);
181 noTone(SIRENA);
182 }
183 else if (*note == 254)
184 {
185 tone(SIRENA, FIS3);
186 delay(music_speed);
187 }
188 else if (*note == 255)
189 {
190 tone(SIRENA, G3);
191 delay(music_speed);
192 }
193 else
194 {
195 uint8_t len = *note / 50;
196 uint16_t d = music_speed;
197 while (len--) d *= 2;
198 uint8_t n = *note % 50;
199 if (n != 49)
200 {
201 uint8_t octave = (n + 5) / 12;
202 n = (n + 5 ) % 12;
203 float freq = octave_4[n];
204 octave = 4 - octave;
205 while (octave > 0)
206 {
207 freq /= 2.0;
208 octave--;
209 }
210 tone(SIRENA, (int)freq);
211 }
212 delay(d);
213 noTone(SIRENA);
214 }
215 }
216
217 void zahraj_melodiu()
218 {
219 music_speed = 800 / 16;
220 for (int j = 0; j < 4; j++)
221 {
222 for (int i = 0; i < dlzka_melodie; i++)
223 {
224 otto_tone(melodia + i);
225 if (melodia[i] == 251) i += 4;
226 }
227 music_speed -= 150 / 16;
228 }
229 delay(3000);
230 }
231
232
233 void test_ultrazvuk()
234 {
235 int i = 0;
236 while ((serial_available() == 0) && (Serial.available() == 0))
237 {
238 serial_println_num(measure_distance());
239 delay(100);
240 }
241 serial_read();
242 }
243
244 void menu_ultrasonic_request()
245 {
246 uint32_t tm = millis();
247 int d = measure_distance();
248 int count = 0;
249 int count2 = 0;
250 while ((millis() - tm < 1500) && ((d < 15) || (count2 < 5)) && (count < 10))
251 {
252 delay(10);
253 d = measure_distance();
254 if (d == 10000) count++;
255 else count = 0;
256 if (d >= 15) count2++;
257 else count2 = 0;
258 }
259 if (millis() - tm >= 1500)
260 ultrasonic_menu();
261 }
262
263 void ultrasonic_menu()
264 {
265 int selection = 0;
266 tone(SIRENA, 880, 200);
267
268 do {
269 int count = 0;
270 do {
271 int32_t d = measure_distance();
272 if (d == 10000) continue;
273 if (d >= 20) count++;
274 else count = 0;
275 delay(10);
276 } while (!serial_available() && !Serial.available() && (count < 20));
277
278 tone(SIRENA, 440, 200);
279 uint32_t tm = millis();
280 while ((measure_distance() > 15) && (millis() - tm < 1500) && !serial_available() && !Serial.available()) delay(10);
281 if (millis() - tm >= 1500)
282 {
283 tone(SIRENA, 2000, 50);
284 menu_command(selection);
285 return;
286 }
287 selection++;
288 for (int i = 0; i < selection; i++)
289 {
290 tone(SIRENA, 1261, 50);
291 delay(250);
292 }
293 } while (!serial_available() && !Serial.available());
294 while (serial_available()) serial_read();
295 while (Serial.available()) Serial.read();
296 }
297
298 void menu_command(int cmd)
299 {
300 if (cmd == 2) zahraj_melodiu();
301 if (cmd == 3) melodia_jedna_druhej();
302 if (cmd == 1) vpred();
303 if (cmd == 4) ahoj();
304 serial_println_num(cmd);
305 }
306
307 void melodia_jedna_druhej()
308 {
309 for (int i = 0; i < 2; i++)
310 {
311 tone(SIRENA, 262, 200);
312 delay(200);
313 tone(SIRENA, 330, 200);
314 delay(200);
315
316 tone(SIRENA, 262, 200);
317 delay(200);
318 tone(SIRENA, 330, 200);
319 delay(200);
320
321 tone(SIRENA, 392, 400);
322 delay(400);
323
324 tone(SIRENA, 392, 400);
325 delay(400);
326 }
327 noTone(8);
328 }
329
330 uint16_t chor_time[] = {100,1,1,100,1,1,1,100,1,1,1,100,1,100,1,0};
331 uint8_t chor_servo[] = {1,2,6,4,3,6,5,1,2,6,5,3,4,1,2,0};
332 uint8_t chor_val[] = {48,69,180,104,104,90,0,111,146,0,90,62,69,48,69,0};
333 uint8_t chor_len = 16;
334
335
336
337 void vpred()
338 {
339 while (measure_distance() > 30)
340 zatancuj_choreografiu(chor_time, chor_servo, chor_val, chor_len);
341 pipni();
342 reset();
343 }
344
345 void precitaj_kalibraciu_z_EEPROM()
346 {
347 uint8_t value = EEPROM.read(1);
348 if (value != '~') return;
349 for (int i = 2; i < 8; i++)
350 prednastavena_kalibracia[i - 2] = EEPROM.read(i);
351 for (int i = 0; i < 6; i++)
352 dolny_limit[i] = EEPROM.read(i + 9);
353 for (int i = 0; i < 6; i++)
354 horny_limit[i] = EEPROM.read(i + 15);
355 }
356
357 char read_char()
358 {
359 while (!serial_available() && !Serial.available());
360 if (serial_available()) return serial_read();
361 else return Serial.read();
362 }
363
364 void zapis_kalibraciu_do_EEPROM()
365 {
366 serial_print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
367 char odpoved = read_char();
368 serial_println_char(odpoved);
369 if (odpoved == 'Y')
370 {
371 EEPROM.write(1, '~');
372 for (int i = 2; i < 8; i++)
373 EEPROM.write(i, kalib[i - 2]);
374 for (int i = 0; i < 6; i++)
375 EEPROM.write(9 + i, dolny_limit[i]);
376 for (int i = 0; i < 6; i++)
377 EEPROM.write(15 + i, horny_limit[i]);
378 serial_println("ok");
379 }
380 }
381
382 void pipni()
383 {
384 tone(SIRENA, 1568, 50);
385 delay(100);
386 tone(SIRENA, 1357, 50);
387 }
388
389 void ruky()
390 {
391 int odloz_krok = krok;
392 delay(500);
393 krok = 90;
394 pohyb(SERVO_LAVA_RUKA);
395 pohyb(SERVO_PRAVA_RUKA);
396 delay(1000);
397 krok = 180;
398 pohyb(-SERVO_LAVA_RUKA);
399 pohyb(-SERVO_PRAVA_RUKA);
400 delay(1000);
401 krok = odloz_krok;
402 pipni();
403 }
404
405 void ruky2()
406 {
407 int odloz_krok = krok;
408 delay(500);
409 krok = 180;
410 pohyb(SERVO_LAVA_RUKA);
411 pohyb(SERVO_PRAVA_RUKA);
412 delay(1000);
413 krok = 90;
414 pohyb(-SERVO_LAVA_RUKA);
415 pohyb(-SERVO_PRAVA_RUKA);
416 delay(1000);
417 krok = odloz_krok;
418 pipni();
419 }
420
421 void ahoj()
422 {
423 tone(SIRENA, 1568, 50);
424 delay(70);
425 tone(SIRENA, 1175, 30);
426 delay(50);
427 tone(SIRENA, 880, 30);
428 delay(50);
429 tone(SIRENA, 1047, 50);
430 delay(70);
431 tone(SIRENA, 1245, 30);
432 delay(150);
433 tone(SIRENA, 1568, 50);
434 delay(100);
435 if (random(10) > 4) tone(SIRENA, 1357, 50);
436 else tone(SIRENA, 1047, 50);
437 }
438
439 void nastav_koncatinu(int8_t servo, uint8_t poloha)
440 {
441 int8_t delta;
442 int8_t srv = (servo > 0)?servo:-servo;
443 srv--;
444 poloha += kalib[srv] - 90;
445 if (poloha > 180) poloha = 180;
446 if (poloha < 0) poloha = 0;
447
448 if (stav[srv] < poloha) delta = 1;
449 else delta = -1;
450 while (stav[srv] != poloha)
451 {
452 stav[srv] +=delta;
453 s[srv].write(stav[srv]);
454 delay(spomalenie);
455 }
456 }
457
458 void zobraz_stav()
459 {
460 for (int i = 0; i < 6; i++)
461 {
462 serial_print("S"); serial_print_num(i + 1); serial_print(": "); serial_println_num(stav[i] - kalib[i] + 90);
463 }
464 serial_println("---");
465 pipni();
466 }
467
468 void pohyb(int8_t servo)
469 {
470 int8_t srv = (servo > 0)?servo:-servo;
471 srv--;
472 if (servo_invertovane[srv]) servo = -servo;
473 if (servo > 0)
474 {
475 if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
476 else stav[srv] = horny_limit[srv];
477 s[srv].write(stav[srv]);
478 }
479 else if (servo < 0)
480 {
481 if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok;
482 else stav[srv] = dolny_limit[srv];
483 s[srv].write(stav[srv]);
484 }
485 }
486
487 uint8_t pohyb_znakom(char z)
488 {
489 for (int i = 0; i < 12; i++)
490 {
491 if (z == znaky_zmien[i])
492 {
493 int8_t servo = zmeny[i];
494 pohyb(servo);
495 }
496 }
497 }
498
499 void kombinacia1()
500 {
501 pohyb(SERVO_LAVA_NOHA);
502 pohyb(-SERVO_PRAVA_PATA);
503 }
504
505 void kombinacia2()
506 {
507 pohyb(SERVO_PRAVA_NOHA);
508 pohyb(-SERVO_LAVA_PATA);
509 }
510
511 void kombinacia3()
512 {
513 pohyb(SERVO_LAVA_RUKA);
514 pohyb(SERVO_PRAVA_RUKA);
515 }
516
517 void kombinacia4()
518 {
519 pohyb(-SERVO_LAVA_RUKA);
520 pohyb(-SERVO_PRAVA_RUKA);
521 }
522
523 int pohyb_kombinacia(char z)
524 {
525 if (z == '3') kombinacia1();
526 else if (z == '4') kombinacia2();
527 else if (z == '5') kombinacia3();
528 else if (z == '6') kombinacia4();
529 else return 0;
530 return 1;
531 }
532
533 int nacitajCislo()
534 {
535 int num = 0;
536 int z;
537 do {
538 z = read_char();
539 if (z == '#') while (z != 13) z = read_char();
540 } while ((z < '0') || (z > '9'));
541 while ((z >= '0') && (z <= '9'))
542 {
543 num *= 10;
544 num += (z - '0');
545 do { z = read_char(); if (z == -1) delayMicroseconds(10); } while (z < 0);
546 }
547 return num;
548 }
549
550 void nacitaj_choreografiu()
551 {
552 ch_len = 0;
553 int tm;
554 do {
555 tm = nacitajCislo();
556 ch_time[ch_len] = tm;
557 ch_servo[ch_len] = nacitajCislo();
558 ch_val[ch_len] = nacitajCislo();
559 ch_len++;
560 if (ch_len == CHOREO_LEN) break;
561 } while (tm > 0);
562 pipni();
563 }
564
565 void vypis_choreografiu()
566 {
567 for (int i = 0; i < ch_len; i++)
568 {
569 serial_print_num(ch_time[i]);
570 serial_print(" ");
571 serial_print_num(ch_servo[i]);
572 serial_print(" ");
573 serial_println_num(ch_val[i]);
574 }
575 pipni();
576 }
577
578 void zatancuj_choreografiu(uint16_t *ch_time, uint8_t *ch_servo, uint8_t *ch_val, int ch_len )
579 {
580 for (int i = 0; i < ch_len - 1; i++)
581 {
582 delay(ch_time[i]);
583 nastav_koncatinu(ch_servo[i], ch_val[i]);
584 }
585 // if (ch_len > 0) stav[ch_servo[ch_len - 1]] = ch_val[ch_len - 1];
586 pipni();
587 while (serial_available()) serial_read();
588 while (Serial.available()) Serial.read();
589 }
590
591 void reset()
592 {
593 for (int i = 0; i < 6; i++)
594 {
595 stav[i] = kalib[i];
596 s[i].write(kalib[i]);
597 delay(300);
598 }
599 pipni();
600 }
601
602 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
603 {
604 serial_print(" (+/-/ENTER): ");
605 serial_println_num(hodnota);
606 s[servo].write(hodnota);
607 char z;
608 do {
609 z = read_char();
610 if ((z == '+') && (hodnota < 180)) hodnota++;
611 else if ((z == '-') && (hodnota > 0)) hodnota--;
612 if ((z == '+') || (z == '-'))
613 {
614 serial_print_num(hodnota); serial_print_char('\r');
615 s[servo].write(hodnota);
616 }
617 } while (z != 13);
618 return hodnota;
619 }
620
621 void kalibruj()
622 {
623 for (int i = 0; i < 6; i++)
624 {
625 serial_print_num(i);
626 kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
627 serial_print_num(i);
628 serial_print(": ");
629 serial_println_num(kalib[i]);
630 }
631 for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
632 serial_println("ok");
633 pipni();
634 }
635
636 void nastav_limity()
637 {
638 for (int i = 0; i < 6; i++)
639 {
640 serial_print_num(i);
641 serial_print("dolny");
642 dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
643 serial_print_num(i);
644 serial_print(" dolny: ");
645 serial_println_num(dolny_limit[i]);
646 s[i].write(kalib[i]);
647
648 serial_print_num(i);
649 serial_print("horny");
650 horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
651 serial_print_num(i);
652 serial_print(" horny: ");
653 serial_println_num(horny_limit[i]);
654 s[i].write(kalib[i]);
655 }
656 for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print("-"); serial_print_num(horny_limit[i]); serial_print(" "); }
657 serial_println("ok");
658 pipni();
659 }
660
661 void vypis_kalibraciu()
662 {
663 serial_print("stredy: ");
664 for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
665 serial_println();
666 serial_print("dolny limit: ");
667 for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print(" "); }
668 serial_println();
669 serial_print("horny limit: ");
670 for (int i = 0; i < 6; i++) { serial_print_num(horny_limit[i]); serial_print(" "); }
671 serial_println();
672 }
673
674 void nacitaj_kalibraciu()
675 {
676 int tm;
677 for (int i = 0; i < 6; i++)
678 kalib[i] = nacitajCislo();
679 vypis_kalibraciu();
680 serial_println("ok");
681 pipni();
682 }
683
684 void zvys_krok()
685 {
686 if (krok < 180) krok++;
687 serial_print("krok: ");
688 serial_println_num(krok);
689 }
690
691 void zniz_krok()
692 {
693 if (krok > 0) krok--;
694 serial_print("krok: ");
695 serial_println_num(krok);
696 }
697
698 void zniz_spomalenie()
699 {
700 if (spomalenie > 0) spomalenie--;
701 serial_print("spomalenie: ");
702 serial_println_num(spomalenie);
703 }
704
705 void zvys_spomalenie()
706 {
707 if (spomalenie < 100) spomalenie++;
708 serial_print("spomalenie: ");
709 serial_println_num(spomalenie);
710 }
711
712 // nasleduje softverova implementacia serioveho portu
713 #define SERIAL_STATE_IDLE 0
714 #define SERIAL_STATE_RECEIVING 1
715 #define SERIAL_BUFFER_LENGTH 20
716
717 static volatile uint8_t serial_state;
718 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
719 static volatile uint8_t serial_buf_wp, serial_buf_rp;
720
721 static volatile uint8_t receiving_byte;
722
723 static volatile uint32_t time_startbit_noticed;
724 static volatile uint8_t next_bit_order;
725 static volatile uint8_t waiting_stop_bit;
726 static uint16_t one_byte_duration;
727 static uint16_t one_bit_duration;
728 static uint16_t one_bit_write_duration;
729 static uint16_t half_of_one_bit_duration;
730
731 void init_serial(uint32_t baud_rate)
732 {
733 pinMode(2, INPUT);
734 pinMode(4, OUTPUT);
735
736 serial_state = SERIAL_STATE_IDLE;
737
738 one_byte_duration = 9500000 / baud_rate;
739 one_bit_duration = 1000000 / baud_rate;
740 one_bit_write_duration = one_bit_duration - 1;
741 half_of_one_bit_duration = 500000 / baud_rate;
742
743 PCMSK2 |= 4; //PCINT18;
744 PCIFR &= ~4; //PCIF2;
745 PCICR |= 4; // PCIE2;
746 }
747
748 ISR(PCINT2_vect)
749 {
750 uint32_t tm = micros();
751 if (serial_state == SERIAL_STATE_IDLE)
752 {
753 time_startbit_noticed = tm;
754 serial_state = SERIAL_STATE_RECEIVING;
755 receiving_byte = 0xFF;
756 next_bit_order = 0;
757 }
758 else if (tm - time_startbit_noticed > one_byte_duration)
759 {
760 serial_buffer[serial_buf_wp] = receiving_byte;
761 serial_buf_wp++;
762 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
763 time_startbit_noticed = tm;
764 receiving_byte = 0xFF;
765 next_bit_order = 0;
766 }
767 else if (PIND & 4)
768 {
769 int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
770 while (next_bit_order < new_next_bit_order)
771 {
772 receiving_byte &= ~(1 << next_bit_order);
773 next_bit_order++;
774 }
775 if (next_bit_order == 8)
776 {
777 serial_buffer[serial_buf_wp] = receiving_byte;
778 serial_buf_wp++;
779 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
780 serial_state = SERIAL_STATE_IDLE;
781 }
782 } else
783 next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
784 }
785
786 uint8_t serial_available()
787 {
788 cli();
789 if (serial_buf_rp != serial_buf_wp)
790 {
791 sei();
792 return 1;
793 }
794 if (serial_state == SERIAL_STATE_RECEIVING)
795 {
796 uint32_t tm = micros();
797 if (tm - time_startbit_noticed > one_byte_duration)
798 {
799 serial_state = SERIAL_STATE_IDLE;
800 serial_buffer[serial_buf_wp] = receiving_byte;
801 serial_buf_wp++;
802 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
803 sei();
804 return 1;
805 }
806 }
807 sei();
808 return 0;
809 }
810
811 int16_t serial_read()
812 {
813 cli();
814 if (serial_buf_rp != serial_buf_wp)
815 {
816 uint8_t ch = serial_buffer[serial_buf_rp];
817 serial_buf_rp++;
818 if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
819 sei();
820 return ch;
821 }
822
823 if (serial_state == SERIAL_STATE_RECEIVING)
824 {
825 uint32_t tm = micros();
826 if (tm - time_startbit_noticed > one_byte_duration)
827 {
828 uint8_t ch = receiving_byte;
829 serial_state = SERIAL_STATE_IDLE;
830 sei();
831 return ch;
832 }
833 }
834 sei();
835 return -1;
836 }
837
838 void serial_write(uint8_t ch)
839 {
840 #ifdef ECHO_BT_TO_USB
841 Serial.print((char)ch);
842 #endif
843 PORTD &= ~16;
844 delayMicroseconds(one_bit_write_duration);
845 for (uint8_t i = 0; i < 8; i++)
846 {
847 if (ch & 1) PORTD |= 16;
848 else PORTD &= ~16;
849 ch >>= 1;
850 delayMicroseconds(one_bit_write_duration);
851 }
852 PORTD |= 16;
853 delayMicroseconds(one_bit_write_duration);
854 delayMicroseconds(one_bit_write_duration);
855 delayMicroseconds(one_bit_write_duration);
856 delayMicroseconds(one_bit_write_duration);
857 delayMicroseconds(one_bit_write_duration);
858 }
859
860 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
861 {
862 uint16_t len;
863 uint16_t ch;
864 do {
865 ch = serial_read();
866 if (ch == 13) continue;
867 } while (ch == -1);
868
869 do {
870 if ((ch != 13) && (ch != 10) && (ch != -1))
871 {
872 *(ln++) = ch;
873 max_length--;
874 len++;
875 }
876 ch = serial_read();
877 } while ((ch != 13) && max_length);
878 *ln = 0;
879 return len;
880 }
881
882 void serial_print_num(int32_t number)
883 {
884 if (number < 0)
885 {
886 serial_write('-');
887 number = -number;
888 }
889 int32_t rad = 1;
890 while (number / rad) rad *= 10;
891 if (number > 0) rad /= 10;
892 while (rad)
893 {
894 serial_write((char)('0' + (number / rad)));
895 number -= (number / rad) * rad;
896 rad /= 10;
897 }
898 }
899
900 void serial_print_char(char ch)
901 {
902 serial_write(ch);
903 }
904
905 void serial_print(const uint8_t *str)
906 {
907 while (*str) serial_write(*(str++));
908 }
909
910 void serial_println(const uint8_t *str)
911 {
912 serial_print(str);
913 serial_write(13);
914 serial_write(10);
915 }
916
917 void serial_println_num(int32_t number)
918 {
919 serial_print_num(number);
920 serial_println();
921 }
922
923 void serial_println_char(char ch)
924 {
925 serial_write(ch);
926 serial_println();
927 }
928
929 void serial_println()
930 {
931 serial_write(13);
932 serial_write(10);
933 }
934
935 // nasleduje citanie z utltazvukoveho senzora
936
937 static volatile uint32_t pulse_start;
938 static volatile uint8_t new_distance;
939
940 void init_ultrasonic()
941 {
942 pinMode(US_ECHO, INPUT);
943 pinMode(US_TRIG, OUTPUT);
944
945 PCMSK0 |= 1; //PCINT0;
946 PCIFR &= ~1; //PCIF0;
947 PCICR |= 1; // PCIE0;
948 }
949
950 ISR(PCINT0_vect)
951 {
952 if (PINB & 1) pulse_start = micros();
953 else
954 {
955 distance = (int16_t)((micros() - pulse_start) / 58);
956 new_distance = 1;
957 }
958 }
959
960 void start_distance_measurement()
961 {
962 distance = 10000;
963 new_distance = 0;
964 digitalWrite(US_TRIG, HIGH);
965 delayMicroseconds(10);
966 digitalWrite(US_TRIG, LOW);
967 }
968
969 void wait_for_distance_measurement_to_complete()
970 {
971 uint8_t counter = 0;
972 while ((counter < 20) && !new_distance)
973 {
974 delay(1);
975 counter++;
976 }
977 if (counter == 20)
978 {
979 pinMode(US_ECHO, OUTPUT);
980 digitalWrite(US_ECHO, HIGH);
981 delayMicroseconds(10);
982 digitalWrite(US_ECHO, LOW);
983 pinMode(US_ECHO, INPUT);
984 delayMicroseconds(5);
985 distance = 10000;
986 }
987 }
988
989 int16_t measure_distance()
990 {
991 start_distance_measurement();
992 wait_for_distance_measurement_to_complete();
993 return distance;
994 }
