Otto - riadiaci program v.3
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, 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 |
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) |
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 // tu su serva cislovane 1-6
35 #define SERVO_LAVA_RUKA 5
36 #define SERVO_PRAVA_RUKA 6
37 #define SERVO_LAVA_NOHA 4
38 #define SERVO_PRAVA_NOHA 3
39 #define SERVO_LAVA_PATA 2
40 #define SERVO_PRAVA_PATA 1
41
42 // ak su niektore serva naopak, je tu jednotka
43 uint8_t servo_invertovane[6] = {0, 0, 1, 1, 0, 1};
44
45 // znaky, ktorymi sa ovladaju jednotlive stupne volnosti
46 char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' };
47 // co robia jednotlive znaky (znamienko urcuje smer)
48 int8_t zmeny[] = {SERVO_LAVA_RUKA, -SERVO_LAVA_RUKA,
49 SERVO_PRAVA_RUKA, -SERVO_PRAVA_RUKA,
50 -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA,
51 -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA,
52 SERVO_LAVA_PATA, -SERVO_LAVA_PATA,
53 SERVO_PRAVA_PATA, -SERVO_PRAVA_PATA };
54
55 // sem si mozno ulozit svoju kalibraciu
56 uint8_t prednastavena_kalibracia[] = { 90, 90, 90, 90, 90, 90 };
57
58 uint8_t dolny_limit[] = {0, 0, 0, 0, 0, 0, 0, 0};
59 uint8_t horny_limit[] = {180, 180, 180, 180, 180, 180};
60
61 Servo s[6];
62
63 uint16_t ch_time[CHOREO_LEN];
64 uint8_t ch_servo[CHOREO_LEN];
65 uint8_t ch_val[CHOREO_LEN];
66 int ch_len;
67 uint8_t kalib[6];
68 int stav[6];
69 int krok;
70 uint8_t spomalenie;
71
72 static volatile int16_t distance;
73
74 #define FIS3 2960
75 #define G3 3136
76
77 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 };
78 int16_t music_speed;
79
80 //popcorn
81 uint16_t dlzka_melodia[] = {0, 386, 26, 281, 217};
82 const uint8_t melodia1[] PROGMEM = { 252, 50, 149, 49,
83 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,
84 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,
85 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,
86 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,
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, 149,
88 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,
89 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,
90 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 ,
91 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,
92 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,
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, 149 ,
94 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
95 };
96
97 //kohutik jarabi
98 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 };
99
100 // kankán
101 const uint8_t melodia3[] PROGMEM = { 252, 100,
102 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,
103 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,
104 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,
105 135,131,128,126,75,76,78,80,81,76,80,76,81,76,80,76,81,76,80,76,81,76,80,76,
106 251,2,11,3,16,19,251,1,4,3,16,19,
107 251,1,4,3,16,19,251,1,4,3,16,19,
108 251,1,4,3,16,19,251,1,4,3,16,19,
109 251,1,4,3,16,19,251,1,4,3,16,19,
110 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,
111 174,88,91,90,88,143,143,93,95,90,91,138,138,88,91,90,88,86,93,89,90,136,
112 64,71,71,73,71,69,69,73,74,78,81,78,78,76,251,1,184,1,32,126,
113 78,76,126,78,76,126,78,76,126,78,76,126,78,76,126,78,76,126,
114 78,76,78,76,78,76,78,76,
115 131,119,119,119,169 };
116
117 //labutie jazero
118 const uint8_t melodia4[] PROGMEM = {
119 252, 220, 66, 69, 73, 69, 66, 69, 73, 69, 66, 69, 73, 69, 66, 69, 73, 69,
120 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,
121 99, 83, 81, 80,
122 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,
123 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,
124 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,
125 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,
126 99, 83, 81, 80,
127 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 };
128
129 void setup() {
130 Serial.begin(9600);
131 init_serial(9600);
132 init_ultrasonic();
133
134 randomSeed(analogRead(1));
135 s[0].attach(PIN_SERVO_PRAVA_PATA);
136 s[1].attach(PIN_SERVO_LAVA_PATA);
137 s[2].attach(PIN_SERVO_PRAVA_NOHA);
138 s[3].attach(PIN_SERVO_LAVA_NOHA);
139 s[4].attach(PIN_SERVO_LAVA_RUKA);
140 s[5].attach(PIN_SERVO_PRAVA_RUKA);
141 precitaj_kalibraciu_z_EEPROM();
142 for (int i = 0; i < 6; i++)
143 {
144 kalib[i] = prednastavena_kalibracia[i];
145 stav[i] = kalib[i];
146 s[i].write(stav[i]);
147 }
148 ch_len = 0;
149 krok = 7;
150 spomalenie = 6;
151 ahoj();
152 ruky2();
153 delay(100);
154 serial_println_flash(PSTR("\r\n Otto DTDT"));
155 }
156
157 void loop() {
158 char z = -1;
159 if (serial_available()) z = serial_read();
160 #ifdef ECHO_BT_TO_USB
161 if (Serial.available()) z = Serial.read();
162 #endif
163
164 if (z != -1)
165 {
166 if (pohyb_znakom(z)) return;
167 else if (pohyb_kombinacia(z)) return;
168 else if (vydaj_zvukovy_efekt(z)) return;
169 else if (ma_zahrat_melodiu(z)) return;
170 else if (z == '@') nacitaj_choreografiu();
171 else if (z == '?') vypis_choreografiu();
172 else if (z == 't') zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
173 else if (z == '-') ahoj();
174 else if (z == ' ') reset();
175 else if (z == 'H') kalibruj();
176 else if (z == 'J') nastav_limity();
177 else if (z == 'G') vypis_kalibraciu();
178 else if (z == 'L') nacitaj_kalibraciu();
179 else if (z == 'E') zapis_kalibraciu_do_EEPROM();
180 else if (z == 'R') ruky();
181 else if (z == '9') zvys_krok();
182 else if (z == '1') zniz_krok();
183 else if (z == '8') zvys_spomalenie();
184 else if (z == '7') zniz_spomalenie();
185 else if (z == 'U') test_ultrazvuk();
186 else if (z == 'S') zobraz_stav();
187 else if (z == 'W') skus_zapisat_choreografiu_do_EEPROM();
188 else if (z == 'X') skus_naformatovat_EEPROM_choreografie();
189 else if (z == 'C') skus_nacitat_choreografiu_z_EEPROM();
190 }
191 int16_t d = measure_distance();
192 if (d < 10) menu_ultrasonic_request();
193 }
194
195 const uint8_t klavesy_efektov[] PROGMEM = {'v', 'b', 'n', 'j', 'h', 'g', 'f', 's', 'l', 'o', 'i', 'u', 'y', 'r', 'e', 'w'};
196 #define POCET_KLAVES_EFEKTOV 16
197
198 uint8_t vydaj_zvukovy_efekt(char z)
199 {
200 for (int i = 0; i < POCET_KLAVES_EFEKTOV; i++)
201 if (z == pgm_read_byte(klavesy_efektov + i))
202 {
203 zvukovy_efekt(i + 1);
204 return 1;
205 }
206 return 0;
207 }
208
209 uint8_t ma_zahrat_melodiu(char z)
210 {
211 if (z == 9) zahraj_melodiu(1);
212 else if (z == '*') zahraj_melodiu(2);
213 else if (z == '/') zahraj_melodiu(3);
214 else if (z == '+') zahraj_melodiu(4);
215 }
216
217 void otto_translate_tone(uint8_t *note, uint16_t *freq, uint16_t *del)
218 {
219 if (*note == 251)
220 {
221 *freq = ((*(note + 1)) << 8) + (*(note + 2));
222 uint8_t d1 = *(note + 3);
223 uint8_t d2 = *(note + 4);
224 *del = (music_speed * 16 * (long)d1) / d2;
225 }
226 else if (*note == 254)
227 {
228 *freq = FIS3;
229 *del = music_speed;
230 }
231 else if (*note == 255)
232 {
233 *freq = G3;
234 *del = music_speed;
235 }
236 else
237 {
238 uint8_t len = *note / 50;
239 *del = music_speed;
240 while (len--) *del *= 2;
241 uint8_t n = *note % 50;
242 if (n != 49)
243 {
244 uint8_t octave = (n + 5) / 12;
245 n = (n + 5 ) % 12;
246 float ffreq = octave_4[n];
247 octave = 4 - octave;
248 while (octave > 0)
249 {
250 ffreq /= 2.0;
251 octave--;
252 }
253 *freq = (uint16_t)ffreq;
254 }
255 else *freq = 0;
256 }
257 }
258
259 void otto_tone(uint8_t *note)
260 {
261 uint16_t freq, d;
262 otto_translate_tone(note, &freq, &d);
263 if (freq) tone(SIRENA, freq); else noTone(SIRENA);
264 delay(d);
265 noTone(SIRENA);
266 }
267
268 void zahraj_melodiu(uint8_t cislo)
269 {
270 uint8_t nota[5];
271 music_speed = 800 / 16;
272 for (int i = 0; i < dlzka_melodia[cislo]; i++)
273 {
274 if (serial_available() || Serial.available()) break;
275 switch(cislo){
276 case 1: nota[0] = pgm_read_byte(melodia1 + i); break;
277 case 2: nota[0] = pgm_read_byte(melodia2 + i); break;
278 case 3: nota[0] = pgm_read_byte(melodia3 + i); break;
279 case 4: nota[0] = pgm_read_byte(melodia4 + i); break;
280 }
281 if (nota[0] == 252)
282 {
283 switch(cislo){
284 case 1: music_speed = pgm_read_byte(melodia1 + i + 1); break;
285 case 2: music_speed = pgm_read_byte(melodia2 + i + 1); break;
286 case 3: music_speed = pgm_read_byte(melodia3 + i + 1); break;
287 case 4: music_speed = pgm_read_byte(melodia4 + i + 1); break;
288 }
289 i++;
290 continue;
291 }
292 if (nota[0] == 251)
293 {
294 for (int k = 1; k < 5; k++)
295 switch(cislo){
296 case 1: nota[k] = pgm_read_byte(melodia1 + i + k); break;
297 case 2: nota[k] = pgm_read_byte(melodia2 + i + k); break;
298 case 3: nota[k] = pgm_read_byte(melodia3 + i + k); break;
299 case 4: nota[k] = pgm_read_byte(melodia4 + i + k); break;
300 }
301 i += 4;
302 }
303 otto_tone(nota);
304 }
305 }
306
307 void efekt1()
308 {
309 uint16_t fre = 100;
310 for (int i = 0; i < 200; i++)
311 {
312 tone(SIRENA, fre);
313 fre += 10;
314 delay(2);
315 }
316 noTone(SIRENA);
317 }
318
319 void efekt2()
320 {
321 uint16_t fre = 2000;
322 for (int i = 0; i < 200; i++)
323 {
324 tone(SIRENA, fre);
325 fre -= 10;
326 delay(2);
327 }
328 noTone(SIRENA);
329 }
330
331 void efekt3()
332 {
333 uint16_t fre;
334 for (int i = 0; i < 200; i++)
335 {
336 fre = random(3000) + 20;
337 tone(SIRENA, fre);
338 delay(2);
339 }
340 noTone(SIRENA);
341 }
342
343 void efekt4()
344 {
345 uint16_t fre = 100;
346 for (int i = 0; i < 200; i++)
347 {
348 (i&1) ? tone(SIRENA, fre) : tone(SIRENA, 2100 - fre);
349 fre += 10;
350 delay(2);
351 }
352 noTone(SIRENA);
353 }
354
355 void efekt5()
356 {
357 uint16_t fre = 100;
358 for (int i = 0; i < 100; i++)
359 {
360 (i&1) ? tone(SIRENA, fre) : tone(SIRENA, 2100 - fre);
361 fre += 20;
362 delay(4);
363 }
364 noTone(SIRENA);
365 }
366
367 void efekt6()
368 {
369 uint16_t d = 12;
370 for (int i = 0; i < 20; i++)
371 {
372 tone(SIRENA, 1760, 10);
373 delay(d);
374 d += 3;
375 }
376 }
377
378 void efekt7()
379 {
380 for (int i = 0; i < 40; i++)
381 {
382 (i&1) ? noTone(SIRENA): tone(SIRENA, 1760);
383 delay(10);
384 }
385 noTone(SIRENA);
386 }
387
388 void efekt8()
389 {
390 uint16_t d = 72;
391 for (int i = 0; i < 20; i++)
392 {
393 tone(SIRENA, 1760, 10);
394 delay(d);
395 d -= 3;
396 }
397 }
398
399 void efekt9()
400 {
401 float fre = 3500;
402 while (fre > 15)
403 {
404 tone(SIRENA, (uint16_t)fre);
405 fre *= 0.97;
406 delay(2);
407 }
408 noTone(SIRENA);
409 }
410
411 void efekt10()
412 {
413 float fre = 55;
414 while (fre < 3000)
415 {
416 tone(SIRENA, (uint16_t)fre);
417 fre *= 1.03;
418 delay(2);
419 }
420 noTone(SIRENA);
421 }
422
423 void efekt11()
424 {
425 uint16_t fre = 3500;
426 uint16_t d = 42;
427 for (int i = 0; i < 20; i++)
428 {
429 tone(SIRENA, fre);
430 fre -= 165;
431 delay(d);
432 d -= 2;
433 }
434 noTone(SIRENA);
435 }
436
437 void efekt12()
438 {
439 float fre = 110;
440 uint8_t d = 42;
441 for (int i = 0; i < 20; i++)
442 {
443 tone(SIRENA, fre);
444 fre += 165;
445 delay(d);
446 d -= 2;
447 }
448 noTone(SIRENA);
449 }
450
451 void efekt13()
452 {
453 uint16_t fre = 3400;
454 uint8_t d = 200;
455 uint8_t delta = 1;
456 for (int i = 0; i < 20; i++)
457 {
458 tone(SIRENA, fre);
459 fre -= 165;
460 delay(d);
461 d -= delta;
462 delta++;
463 }
464 tone(SIRENA, 110);
465 delay(1000);
466 noTone(SIRENA);
467 }
468
469 void efekt14()
470 {
471 uint16_t fre = 880;
472 int16_t d = 20;
473 for (int i = 0; i < 20; i++)
474 {
475 tone(SIRENA, fre);
476 delay(d);
477 fre += random(50) - 25;
478 d += random(10) - 5;
479 if (d < 0) d = 1;
480 }
481 noTone(SIRENA);
482 }
483
484 void efekt15()
485 {
486 uint16_t fre = 440;
487 int16_t d = 20;
488 for (int i = 0; i < 20; i++)
489 {
490 tone(SIRENA, fre);
491 delay(d);
492 fre += random(25) - 12;
493 d += random(10) - 5;
494 if (d < 0) d = 1;
495 }
496 noTone(SIRENA);
497 }
498
499 void efekt16()
500 {
501 uint16_t fre = 1760;
502 int16_t d = 20;
503 for (int i = 0; i < 20; i++)
504 {
505 tone(SIRENA, fre);
506 delay(d);
507 fre += random(100) - 50;
508 d += random(10) - 5;
509 if (d < 0) d = 1;
510 }
511 noTone(SIRENA);
512 }
513
514 void zvukovy_efekt(uint8_t cislo)
515 {
516 switch (cislo) {
517 case 1: efekt1(); break;
518 case 2: efekt2(); break;
519 case 3: efekt3(); break;
520 case 4: efekt4(); break;
521 case 5: efekt5(); break;
522 case 6: efekt6(); break;
523 case 7: efekt7(); break;
524 case 8: efekt8(); break;
525 case 9: efekt9(); break;
526 case 10: efekt10(); break;
527 case 11: efekt11(); break;
528 case 12: efekt12(); break;
529 case 13: efekt13(); break;
530 case 14: efekt14(); break;
531 case 15: efekt15(); break;
532 case 16: efekt16(); break;
533 }
534 }
535
536 void test_ultrazvuk()
537 {
538 int i = 0;
539 while ((serial_available() == 0) && (Serial.available() == 0))
540 {
541 serial_println_num(measure_distance());
542 delay(100);
543 }
544 serial_read();
545 }
546
547 void menu_ultrasonic_request()
548 {
549 uint32_t tm = millis();
550 int d = measure_distance();
551 int count = 0;
552 int count2 = 0;
553 while ((millis() - tm < 1500) && ((d < 15) || (count2 < 5)) && (count < 10))
554 {
555 delay(10);
556 d = measure_distance();
557 if (d == 10000) count++;
558 else count = 0;
559 if (d >= 15) count2++;
560 else count2 = 0;
561 if (serial_available() || Serial.available()) return;
562 }
563 if (millis() - tm >= 1500)
564 ultrasonic_menu();
565 }
566
567 void ultrasonic_menu()
568 {
569 int selection = 0;
570 tone(SIRENA, 880, 200);
571
572 do {
573 int count = 0;
574 do {
575 int32_t d = measure_distance();
576 if (d == 10000) continue;
577 if (d >= 20) count++;
578 else count = 0;
579 delay(10);
580 } while (!serial_available() && !Serial.available() && (count < 20));
581
582 tone(SIRENA, 440, 200);
583 uint32_t tm = millis();
584 while ((measure_distance() > 15) && (millis() - tm < 1500) && !serial_available() && !Serial.available()) delay(10);
585 if (millis() - tm >= 1500)
586 {
587 tone(SIRENA, 2000, 50);
588 menu_command(selection);
589 return;
590 }
591 selection++;
592 for (int i = 0; i < selection; i++)
593 {
594 tone(SIRENA, 1261, 50);
595 delay(250);
596 }
597 } while (!serial_available() && !Serial.available());
598 while (serial_available()) serial_read();
599 while (Serial.available()) Serial.read();
600 }
601
602 void menu_command(int cmd)
603 {
604 if (cmd == 1) vpred();
605 if (cmd == 2) zahraj_melodiu(1);
606 if (cmd == 3) melodia_jedna_druhej();
607 if (cmd == 4) ahoj();
608 if (cmd == 5) zahraj_melodiu(2);
609 if (cmd == 6) zahraj_melodiu(3);
610 if (cmd == 7) zahraj_melodiu(4);
611 serial_println_num(cmd);
612 }
613
614 void melodia_jedna_druhej()
615 {
616 for (int i = 0; i < 2; i++)
617 {
618 tone(SIRENA, 262, 200);
619 delay(200);
620 tone(SIRENA, 330, 200);
621 delay(200);
622
623 tone(SIRENA, 262, 200);
624 delay(200);
625 tone(SIRENA, 330, 200);
626 delay(200);
627
628 tone(SIRENA, 392, 400);
629 delay(400);
630
631 tone(SIRENA, 392, 400);
632 delay(400);
633 }
634 noTone(8);
635 }
636
637 // chodza pre vacsieho dreveneho robota
638 uint16_t chor_time[] = {100,1,1,100,1,1,1,100,1,1,1,100,1,100,1,0};
639 uint8_t chor_servo[] = {1,2,6,4,3,6,5,1,2,6,5,3,4,1,2,0};
640 uint8_t chor_val[] = {48,69,180,104,104,90,0,111,146,0,90,62,69,48,69,0};
641 uint8_t chor_len = 16;
642
643 void vpred()
644 {
645 while (measure_distance() > 30)
646 zatancuj_choreografiu(chor_time, chor_servo, chor_val, chor_len);
647 pipni();
648 reset();
649 }
650
651 void skus_zapisat_choreografiu_do_EEPROM()
652 {
653 serial_print_flash(PSTR("Cislo? [1-3]: "));
654 char odpoved = read_char();
655 if ((odpoved < '1') || (odpoved > '3')) return;
656 serial_println_char(odpoved);
657 uint8_t cislo = odpoved - '0';
658
659 serial_print_flash(PSTR("Zapisat choreografiu do EEPROM c."));
660 serial_print_char(odpoved);
661 serial_print_flash(PSTR("? [Y/n]: "));
662 odpoved = read_char();
663 serial_println_char(odpoved);
664 if (odpoved == 'Y')
665 zapis_choreografiu_do_EEPROM(cislo);
666 }
667
668 void skus_nacitat_choreografiu_z_EEPROM()
669 {
670 serial_print_flash(PSTR("Cislo? [1-3]: "));
671 char odpoved = read_char();
672 if ((odpoved < '1') || (odpoved > '3')) return;
673 serial_println_char(odpoved);
674 uint8_t cislo = odpoved - '0';
675
676 serial_print_flash(PSTR("Precitat choreografiu z EEPROM c."));
677 serial_print_char(odpoved);
678 serial_print_flash(PSTR("? [Y/n]: "));
679 odpoved = read_char();
680 serial_println_char(odpoved);
681 if (odpoved == 'Y')
682 precitaj_choreografiu_z_EEPROM(cislo);
683 }
684
685 void skus_naformatovat_EEPROM_choreografie()
686 {
687 serial_print_flash(PSTR("Formatovat EEPROM choreografii? [Y/n]:"));
688 char odpoved = read_char();
689 serial_println_char(odpoved);
690 if (odpoved == 'Y')
691 naformatuj_EEPROM_choreografie();
692 }
693
694 //EEPROM MAP:
695 // 0: slot number where choreography 3 starts (B3. Real address = B3 x 4)
696 // 1: marker '~' indicates calibration is already stored in EEPROM and will be loaded on startup
697 // 2-8: servo calibration (central points to be used instead of default 90)
698 // 9-14: lower limit for direct control for all 6 servos
699 // 15-20: upper limit for direct control for all 6 servos
700 // 21: number of steps in choreography 1 (L1) 0=choreography not in memory
701 // 22: number of steps in choreography 2 (L2) 0=not in memory
702 // 23: number of steps in choreography 3 (L3) 0=not in memory
703 // 24..(L1 x 4 + 23) choreography1 tuplets (uint16,uint8,uint8) x L1
704 // (1024 - L2 x 4)..1023 choreography2 same as above
705 // B3 x 4..(B3 x 4 + L3 x 4 - 1) choreography 3 same as above
706 void zapis_choreografiu_do_EEPROM(int slot)
707 {
708 uint8_t b3 = EEPROM.read(0);
709 uint8_t len1 = EEPROM.read(21);
710 uint8_t len2 = EEPROM.read(22);
711 uint8_t len3 = EEPROM.read(23);
712 uint16_t wp = 65535;
713
714 if ((len1 > CHOREO_LEN) || (len2 > CHOREO_LEN) || (len3 > CHOREO_LEN) || (b3 > 250 - len3) || ((len1 > b3) && (b3 != 0)) || (len3 + b3 > 250 - len2) || (len1 + len2 > 250))
715 b3 = len1 = len2 = len3 = 0;
716
717 if (slot == 1)
718 {
719 if (((ch_len < b3) || (len3 == 0)) && (ch_len + len2 + len3 <= 250))
720 {
721 EEPROM.write(21, ch_len);
722 wp = 24;
723 }
724 }
725 else if (slot == 2)
726 {
727 if ((250 - b3 - len3 > ch_len) && (ch_len + len1 + len3 <= 250))
728 {
729 EEPROM.write(22, ch_len);
730 wp = 1000 - ch_len * 4;
731 }
732 }
733 else if (slot == 3)
734 {
735 if (ch_len + len1 + len2 <= 250)
736 {
737 EEPROM.write(23, ch_len);
738 EEPROM.write(0, (len1 - len2) / 2 + 125);
739 wp = 4 * ((len1 - len2) / 2 + 125);
740 }
741 }
742
743 if (wp == 65535)
744 serial_println_flash(PSTR("not enough space"));
745 else
746 {
747 for (int i = 0; i < ch_len; i++)
748 {
749 EEPROM.write(wp + 4 * i, ch_time[i] & 255);
750 EEPROM.write(wp + 1 + 4 * i, ch_time[i] >> 8);
751 EEPROM.write(wp + 2 + 4 * i, ch_servo[i]);
752 EEPROM.write(wp + 3 + 4 * i, ch_val[i]);
753 }
754 serial_println_flash(PSTR("ok"));
755 }
756 }
757
758 void precitaj_choreografiu_z_EEPROM(uint8_t slot)
759 {
760 uint8_t b3 = EEPROM.read(0);
761 uint8_t len1 = EEPROM.read(21);
762 uint8_t len2 = EEPROM.read(22);
763 uint8_t len3 = EEPROM.read(23);
764 uint16_t rp = 65535;
765 if ((len1 > CHOREO_LEN) || (len2 > CHOREO_LEN) || (len3 > CHOREO_LEN) || (b3 > 250 - len3) || ((len1 > b3) && (b3 != 0)) || (len3 + b3 > 250 - len2) || (len1 + len2 > 250))
766 b3 = len1 = len2 = len3 = 0;
767
768 if (slot == 1)
769 {
770 if (len1 > 0)
771 {
772 rp = 24;
773 ch_len = len1;
774 }
775 }
776 else if (slot == 2)
777 {
778 if (len2 > 0)
779 {
780 ch_len = len2;
781 rp = 1000 - ch_len * 4;
782 }
783 }
784 else if (slot == 3)
785 {
786 if (len3 > 0)
787 {
788 rp = b3 * 4;
789 ch_len = len3;
790 }
791 }
792
793 if (rp == 65535)
794 serial_println_flash(PSTR("couldn't"));
795 else
796 {
797 for (int i = 0; i < ch_len; i++)
798 {
799 ch_time[i] = ((uint16_t)EEPROM.read(rp + 4 * i)) |
800 (((uint16_t)EEPROM.read(rp + 1 + 4 * i)) << 8);
801 ch_servo[i] = EEPROM.read(rp + 2 + 4 * i);
802 ch_val[i] = EEPROM.read(rp + 3 + 4 * i);
803 }
804 serial_println_flash(PSTR("ok"));
805 pipni();
806 }
807 }
808
809 void naformatuj_EEPROM_choreografie()
810 {
811 EEPROM.write(0, 0);
812 EEPROM.write(21, 0);
813 EEPROM.write(22, 0);
814 EEPROM.write(23, 0);
815 serial_println_flash(PSTR("ok"));
816 pipni();
817 }
818
819 void precitaj_kalibraciu_z_EEPROM()
820 {
821 uint8_t value = EEPROM.read(1);
822 if (value != '~') return;
823 for (int i = 2; i < 8; i++)
824 prednastavena_kalibracia[i - 2] = EEPROM.read(i);
825 for (int i = 0; i < 6; i++)
826 dolny_limit[i] = EEPROM.read(i + 9);
827 for (int i = 0; i < 6; i++)
828 horny_limit[i] = EEPROM.read(i + 15);
829 }
830
831 char read_char()
832 {
833 while (!serial_available() && !Serial.available());
834 if (serial_available()) return serial_read();
835 else return Serial.read();
836 }
837
838 void zapis_kalibraciu_do_EEPROM()
839 {
840 serial_print_flash(PSTR("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: "));
841 char odpoved = read_char();
842 serial_println_char(odpoved);
843 if (odpoved == 'Y')
844 {
845 EEPROM.write(1, '~');
846 for (int i = 2; i < 8; i++)
847 EEPROM.write(i, kalib[i - 2]);
848 for (int i = 0; i < 6; i++)
849 EEPROM.write(9 + i, dolny_limit[i]);
850 for (int i = 0; i < 6; i++)
851 EEPROM.write(15 + i, horny_limit[i]);
852 serial_println_flash(PSTR("ok"));
853 }
854 }
855
856 void pipni()
857 {
858 tone(SIRENA, 1568, 50);
859 delay(100);
860 tone(SIRENA, 1357, 50);
861 }
862
863 void ruky()
864 {
865 int odloz_krok = krok;
866 delay(500);
867 krok = 90;
868 pohyb(SERVO_LAVA_RUKA);
869 pohyb(SERVO_PRAVA_RUKA);
870 delay(1000);
871 krok = 180;
872 pohyb(-SERVO_LAVA_RUKA);
873 pohyb(-SERVO_PRAVA_RUKA);
874 delay(1000);
875 krok = odloz_krok;
876 pipni();
877 }
878
879 void ruky2()
880 {
881 int odloz_krok = krok;
882 delay(500);
883 krok = 180;
884 pohyb(SERVO_LAVA_RUKA);
885 pohyb(SERVO_PRAVA_RUKA);
886 delay(1000);
887 krok = 90;
888 pohyb(-SERVO_LAVA_RUKA);
889 pohyb(-SERVO_PRAVA_RUKA);
890 delay(1000);
891 krok = odloz_krok;
892 pipni();
893 }
894
895 void ahoj()
896 {
897 tone(SIRENA, 1568, 50);
898 delay(70);
899 tone(SIRENA, 1175, 30);
900 delay(50);
901 tone(SIRENA, 880, 30);
902 delay(50);
903 tone(SIRENA, 1047, 50);
904 delay(70);
905 tone(SIRENA, 1245, 30);
906 delay(150);
907 tone(SIRENA, 1568, 50);
908 delay(100);
909 if (random(10) > 4) tone(SIRENA, 1357, 50);
910 else tone(SIRENA, 1047, 50);
911 }
912
913 void nastav_koncatinu(int8_t servo, uint8_t poloha)
914 {
915 int8_t delta;
916 int8_t srv = (servo > 0)?servo:-servo;
917 srv--;
918 poloha += kalib[srv] - 90;
919 if (poloha > 180) poloha = 180;
920 if (poloha < 0) poloha = 0;
921
922 if (stav[srv] < poloha) delta = 1;
923 else delta = -1;
924 while (stav[srv] != poloha)
925 {
926 stav[srv] +=delta;
927 s[srv].write(stav[srv]);
928 delay(spomalenie);
929 }
930 }
931
932 void zobraz_stav()
933 {
934 for (int i = 0; i < 6; i++)
935 {
936 serial_print_flash(PSTR("S")); serial_print_num(i + 1); serial_print_flash(PSTR(": ")); serial_println_num(stav[i] - kalib[i] + 90);
937 }
938 serial_println_flash(PSTR("---"));
939 pipni();
940 }
941
942 void pohyb(int8_t servo)
943 {
944 int8_t srv = (servo > 0)?servo:-servo;
945 srv--;
946 if (servo_invertovane[srv]) servo = -servo;
947 if (servo > 0)
948 {
949 if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
950 else stav[srv] = horny_limit[srv];
951 s[srv].write(stav[srv]);
952 }
953 else if (servo < 0)
954 {
955 if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok;
956 else stav[srv] = dolny_limit[srv];
957 s[srv].write(stav[srv]);
958 }
959 }
960
961 uint8_t pohyb_znakom(char z)
962 {
963 for (int i = 0; i < 12; i++)
964 {
965 if (z == znaky_zmien[i])
966 {
967 int8_t servo = zmeny[i];
968 pohyb(servo);
969 }
970 }
971 }
972
973 void kombinacia1()
974 {
975 pohyb(SERVO_LAVA_NOHA);
976 pohyb(-SERVO_PRAVA_PATA);
977 }
978
979 void kombinacia2()
980 {
981 pohyb(SERVO_PRAVA_NOHA);
982 pohyb(-SERVO_LAVA_PATA);
983 }
984
985 void kombinacia3()
986 {
987 pohyb(SERVO_LAVA_RUKA);
988 pohyb(SERVO_PRAVA_RUKA);
989 }
990
991 void kombinacia4()
992 {
993 pohyb(-SERVO_LAVA_RUKA);
994 pohyb(-SERVO_PRAVA_RUKA);
995 }
996
997 int pohyb_kombinacia(char z)
998 {
999 if (z == '3') kombinacia1();
1000 else if (z == '4') kombinacia2();
1001 else if (z == '5') kombinacia3();
1002 else if (z == '6') kombinacia4();
1003 else return 0;
1004 return 1;
1005 }
1006
1007 int nacitajCislo()
1008 {
1009 int num = 0;
1010 int z;
1011 do {
1012 z = read_char();
1013 if (z == '#') while (z != 13) z = read_char();
1014 } while ((z < '0') || (z > '9'));
1015 while ((z >= '0') && (z <= '9'))
1016 {
1017 num *= 10;
1018 num += (z - '0');
1019 do { z = read_char(); if (z == -1) delayMicroseconds(10); } while (z < 0);
1020 }
1021 return num;
1022 }
1023
1024 void nacitaj_choreografiu()
1025 {
1026 ch_len = 0;
1027 int tm;
1028 do {
1029 tm = nacitajCislo();
1030 ch_time[ch_len] = tm;
1031 ch_servo[ch_len] = nacitajCislo();
1032 ch_val[ch_len] = nacitajCislo();
1033 ch_len++;
1034 if (ch_len == CHOREO_LEN) break;
1035 } while (tm > 0);
1036 pipni();
1037 }
1038
1039 void vypis_choreografiu()
1040 {
1041 for (int i = 0; i < ch_len; i++)
1042 {
1043 serial_print_num(ch_time[i]);
1044 serial_print(" ");
1045 serial_print_num(ch_servo[i]);
1046 serial_print(" ");
1047 serial_println_num(ch_val[i]);
1048 }
1049 serial_println_flash(PSTR("---"));
1050 pipni();
1051 }
1052
1053 void zatancuj_choreografiu(uint16_t *ch_time, uint8_t *ch_servo, uint8_t *ch_val, int ch_len )
1054 {
1055 for (int i = 0; i < ch_len - 1; i++)
1056 {
1057 delay(ch_time[i]);
1058 if (ch_servo[i] < 9) nastav_koncatinu(ch_servo[i], ch_val[i]);
1059 else i = specialny_prikaz(ch_servo[i], ch_val[i]);
1060 if (serial_available() || Serial.available()) break;
1061 }
1062 pipni();
1063 while (serial_available()) serial_read();
1064 while (Serial.available()) Serial.read();
1065 }
1066
1067 int specialny_prikaz(uint8_t prikaz, uint8_t argument)
1068 {
1069 if (prikaz == 9) return ((int)argument) - 1;
1070 else if (prikaz == 10) spomalenie = argument;
1071 else if (prikaz == 11) zacni_hrat_melodiu(argument);
1072 else if (prikaz == 12) zvukovy_efekt(argument);
1073 }
1074
1075 void zacni_hrat_melodiu(uint8_t cislo)
1076 {
1077
1078 }
1079
1080 void reset()
1081 {
1082 for (int i = 0; i < 6; i++)
1083 {
1084 stav[i] = kalib[i];
1085 s[i].write(kalib[i]);
1086 delay(300);
1087 }
1088 pipni();
1089 }
1090
1091 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
1092 {
1093 serial_print_flash(PSTR(" (+/-/ENTER): "));
1094 serial_println_num(hodnota);
1095 s[servo].write(hodnota);
1096 char z;
1097 do {
1098 z = read_char();
1099 if ((z == '+') && (hodnota < 180)) hodnota++;
1100 else if ((z == '-') && (hodnota > 0)) hodnota--;
1101 if ((z == '+') || (z == '-'))
1102 {
1103 serial_print_num(hodnota); serial_print_char('\r');
1104 s[servo].write(hodnota);
1105 }
1106 } while (z != 13);
1107 return hodnota;
1108 }
1109
1110 void kalibruj()
1111 {
1112 for (int i = 0; i < 6; i++)
1113 {
1114 serial_print_num(i);
1115 kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
1116 serial_print_num(i);
1117 serial_print(": ");
1118 serial_println_num(kalib[i]);
1119 }
1120 for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
1121 serial_println_flash(PSTR("ok"));
1122 pipni();
1123 }
1124
1125 void nastav_limity()
1126 {
1127 for (int i = 0; i < 6; i++)
1128 {
1129 serial_print_num(i);
1130 serial_print_flash(PSTR("dolny"));
1131 dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
1132 serial_print_num(i);
1133 serial_print_flash(PSTR(" dolny: "));
1134 serial_println_num(dolny_limit[i]);
1135 s[i].write(kalib[i]);
1136
1137 serial_print_num(i);
1138 serial_print_flash(PSTR("horny"));
1139 horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
1140 serial_print_num(i);
1141 serial_print_flash(PSTR(" horny: "));
1142 serial_println_num(horny_limit[i]);
1143 s[i].write(kalib[i]);
1144 }
1145 for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print("-"); serial_print_num(horny_limit[i]); serial_print(" "); }
1146 serial_println_flash(PSTR("ok"));
1147 pipni();
1148 }
1149
1150 void vypis_kalibraciu()
1151 {
1152 serial_print_flash(PSTR("stredy: "));
1153 for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
1154 serial_println();
1155 serial_print_flash(PSTR("dolny limit: "));
1156 for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print(" "); }
1157 serial_println();
1158 serial_print_flash(PSTR("horny limit: "));
1159 for (int i = 0; i < 6; i++) { serial_print_num(horny_limit[i]); serial_print(" "); }
1160 serial_println();
1161 }
1162
1163 void nacitaj_kalibraciu()
1164 {
1165 int tm;
1166 for (int i = 0; i < 6; i++)
1167 kalib[i] = nacitajCislo();
1168 vypis_kalibraciu();
1169 serial_println_flash(PSTR("ok"));
1170 pipni();
1171 }
1172
1173 void zvys_krok()
1174 {
1175 if (krok < 180) krok++;
1176 serial_print_flash(PSTR("krok: "));
1177 serial_println_num(krok);
1178 }
1179
1180 void zniz_krok()
1181 {
1182 if (krok > 0) krok--;
1183 serial_print_flash(PSTR("krok: "));
1184 serial_println_num(krok);
1185 }
1186
1187 void zniz_spomalenie()
1188 {
1189 if (spomalenie > 0) spomalenie--;
1190 serial_print_flash(PSTR("spomalenie: "));
1191 serial_println_num(spomalenie);
1192 }
1193
1194 void zvys_spomalenie()
1195 {
1196 if (spomalenie < 100) spomalenie++;
1197 serial_print_flash(PSTR("spomalenie: "));
1198 serial_println_num(spomalenie);
1199 }
1200
1201 // nasleduje softverova implementacia serioveho portu
1202 #define SERIAL_STATE_IDLE 0
1203 #define SERIAL_STATE_RECEIVING 1
1204 #define SERIAL_BUFFER_LENGTH 20
1205
1206 static volatile uint8_t serial_state;
1207 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
1208 static volatile uint8_t serial_buf_wp, serial_buf_rp;
1209
1210 static volatile uint8_t receiving_byte;
1211
1212 static volatile uint32_t time_startbit_noticed;
1213 static volatile uint8_t next_bit_order;
1214 static volatile uint8_t waiting_stop_bit;
1215 static uint16_t one_byte_duration;
1216 static uint16_t one_bit_duration;
1217 static uint16_t one_bit_write_duration;
1218 static uint16_t half_of_one_bit_duration;
1219
1220 void init_serial(uint32_t baud_rate)
1221 {
1222 pinMode(2, INPUT);
1223 pinMode(4, OUTPUT);
1224
1225 serial_state = SERIAL_STATE_IDLE;
1226
1227 one_byte_duration = 9500000 / baud_rate;
1228 one_bit_duration = 1000000 / baud_rate;
1229 one_bit_write_duration = one_bit_duration - 1;
1230 half_of_one_bit_duration = 500000 / baud_rate;
1231
1232 PCMSK2 |= 4; //PCINT18;
1233 PCIFR &= ~4; //PCIF2;
1234 PCICR |= 4; // PCIE2;
1235 }
1236
1237 ISR(PCINT2_vect)
1238 {
1239 uint32_t tm = micros();
1240 if (serial_state == SERIAL_STATE_IDLE)
1241 {
1242 time_startbit_noticed = tm;
1243 serial_state = SERIAL_STATE_RECEIVING;
1244 receiving_byte = 0xFF;
1245 next_bit_order = 0;
1246 }
1247 else if (tm - time_startbit_noticed > one_byte_duration)
1248 {
1249 serial_buffer[serial_buf_wp] = receiving_byte;
1250 serial_buf_wp++;
1251 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1252 time_startbit_noticed = tm;
1253 receiving_byte = 0xFF;
1254 next_bit_order = 0;
1255 }
1256 else if (PIND & 4)
1257 {
1258 int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
1259 while (next_bit_order < new_next_bit_order)
1260 {
1261 receiving_byte &= ~(1 << next_bit_order);
1262 next_bit_order++;
1263 }
1264 if (next_bit_order == 8)
1265 {
1266 serial_buffer[serial_buf_wp] = receiving_byte;
1267 serial_buf_wp++;
1268 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1269 serial_state = SERIAL_STATE_IDLE;
1270 }
1271 } else
1272 next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
1273 }
1274
1275 uint8_t serial_available()
1276 {
1277 cli();
1278 if (serial_buf_rp != serial_buf_wp)
1279 {
1280 sei();
1281 return 1;
1282 }
1283 if (serial_state == SERIAL_STATE_RECEIVING)
1284 {
1285 uint32_t tm = micros();
1286 if (tm - time_startbit_noticed > one_byte_duration)
1287 {
1288 serial_state = SERIAL_STATE_IDLE;
1289 serial_buffer[serial_buf_wp] = receiving_byte;
1290 serial_buf_wp++;
1291 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1292 sei();
1293 return 1;
1294 }
1295 }
1296 sei();
1297 return 0;
1298 }
1299
1300 int16_t serial_read()
1301 {
1302 cli();
1303 if (serial_buf_rp != serial_buf_wp)
1304 {
1305 uint8_t ch = serial_buffer[serial_buf_rp];
1306 serial_buf_rp++;
1307 if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
1308 sei();
1309 return ch;
1310 }
1311
1312 if (serial_state == SERIAL_STATE_RECEIVING)
1313 {
1314 uint32_t tm = micros();
1315 if (tm - time_startbit_noticed > one_byte_duration)
1316 {
1317 uint8_t ch = receiving_byte;
1318 serial_state = SERIAL_STATE_IDLE;
1319 sei();
1320 return ch;
1321 }
1322 }
1323 sei();
1324 return -1;
1325 }
1326
1327 void serial_write(uint8_t ch)
1328 {
1329 #ifdef ECHO_BT_TO_USB
1330 Serial.print((char)ch);
1331 #endif
1332 PORTD &= ~16;
1333 delayMicroseconds(one_bit_write_duration);
1334 for (uint8_t i = 0; i < 8; i++)
1335 {
1336 if (ch & 1) PORTD |= 16;
1337 else PORTD &= ~16;
1338 ch >>= 1;
1339 delayMicroseconds(one_bit_write_duration);
1340 }
1341 PORTD |= 16;
1342 delayMicroseconds(one_bit_write_duration);
1343 delayMicroseconds(one_bit_write_duration);
1344 delayMicroseconds(one_bit_write_duration);
1345 delayMicroseconds(one_bit_write_duration);
1346 delayMicroseconds(one_bit_write_duration);
1347 }
1348
1349 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
1350 {
1351 uint16_t len;
1352 uint16_t ch;
1353 do {
1354 ch = serial_read();
1355 if (ch == 13) continue;
1356 } while (ch == -1);
1357
1358 do {
1359 if ((ch != 13) && (ch != 10) && (ch != -1))
1360 {
1361 *(ln++) = ch;
1362 max_length--;
1363 len++;
1364 }
1365 ch = serial_read();
1366 } while ((ch != 13) && max_length);
1367 *ln = 0;
1368 return len;
1369 }
1370
1371 void serial_print_num(int32_t number)
1372 {
1373 if (number < 0)
1374 {
1375 serial_write('-');
1376 number = -number;
1377 }
1378 int32_t rad = 1;
1379 while (number / rad) rad *= 10;
1380 if (number > 0) rad /= 10;
1381 while (rad)
1382 {
1383 serial_write((char)('0' + (number / rad)));
1384 number -= (number / rad) * rad;
1385 rad /= 10;
1386 }
1387 }
1388
1389 void serial_print_char(char ch)
1390 {
1391 serial_write(ch);
1392 }
1393
1394 void serial_print(const uint8_t *str)
1395 {
1396 while (*str) serial_write(*(str++));
1397 }
1398
1399 void serial_println(const uint8_t *str)
1400 {
1401 serial_print(str);
1402 serial_write(13);
1403 serial_write(10);
1404 }
1405
1406 void serial_print_flash(const uint8_t PROGMEM *str)
1407 {
1408 int ln = strlen_P(str);
1409 for (int i = 0; i < ln; i++)
1410 serial_write(pgm_read_byte(str + i));
1411 }
1412
1413 void serial_println_flash(const uint8_t PROGMEM *str)
1414 {
1415 serial_print_flash(str);
1416 serial_write(13);
1417 serial_write(10);
1418 }
1419
1420 void serial_println_num(int32_t number)
1421 {
1422 serial_print_num(number);
1423 serial_println();
1424 }
1425
1426 void serial_println_char(char ch)
1427 {
1428 serial_write(ch);
1429 serial_println();
1430 }
1431
1432 void serial_println()
1433 {
1434 serial_write(13);
1435 serial_write(10);
1436 }
1437
1438 // nasleduje citanie z utltazvukoveho senzora
1439
1440 static volatile uint32_t pulse_start;
1441 static volatile uint8_t new_distance;
1442
1443 void init_ultrasonic()
1444 {
1445 pinMode(US_ECHO, INPUT);
1446 pinMode(US_TRIG, OUTPUT);
1447
1448 PCMSK0 |= 1; //PCINT0;
1449 PCIFR &= ~1; //PCIF0;
1450 PCICR |= 1; // PCIE0;
1451 }
1452
1453 ISR(PCINT0_vect)
1454 {
1455 if (PINB & 1) pulse_start = micros();
1456 else
1457 {
1458 distance = (int16_t)((micros() - pulse_start) / 58);
1459 new_distance = 1;
1460 }
1461 }
1462
1463 void start_distance_measurement()
1464 {
1465 distance = 10000;
1466 new_distance = 0;
1467 digitalWrite(US_TRIG, HIGH);
1468 delayMicroseconds(10);
1469 digitalWrite(US_TRIG, LOW);
1470 }
1471
1472 void wait_for_distance_measurement_to_complete()
1473 {
1474 uint8_t counter = 0;
1475 while ((counter < 20) && !new_distance)
1476 {
1477 delay(1);
1478 counter++;
1479 }
1480 if (counter == 20)
1481 {
1482 pinMode(US_ECHO, OUTPUT);
1483 digitalWrite(US_ECHO, HIGH);
1484 delayMicroseconds(10);
1485 digitalWrite(US_ECHO, LOW);
1486 pinMode(US_ECHO, INPUT);
1487 delayMicroseconds(5);
1488 distance = 10000;
1489 }
1490 }
1491
1492 int16_t measure_distance()
1493 {
1494 start_distance_measurement();
1495 wait_for_distance_measurement_to_complete();
1496 return distance;
1497 }