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