Difference between revisions of "Otto - riadiaci program v.3"
Line 254: | Line 254: | ||
*freq = (uint16_t)ffreq; | *freq = (uint16_t)ffreq; | ||
} | } | ||
− | else * | + | else *freq = 0; |
} | } | ||
} | } |
Revision as of 13:34, 9 August 2018
Program umožňuje priame riadenie robota (aj cez BlueTooth). Teraz už funguje aj na pinoch 2,4 a môže sa naraz programovať aj komunikovať.
Na komunikáciu s robotom môžete použiť napr. program Putty: putty.exe.
klaves | servo | pin | číslo serva v choreografii |
---|---|---|---|
a/q | ľavá ruka | 10 | 5 |
;/p | pravá ruka | 11 | 6 |
z/x | ľavá noha | 9 | 4 |
,/. | pravá noha | 6 | 3 |
d/c | ľavá päta | 5 | 2 |
k/m | pravá päta | 3 | 1 |
1/9 | zníž/zvýš rýchlosť pohybu | - | |
TXD | BT | 2 | |
RXD | BT | 4 | |
TRIG | UZ | 7 | |
ECHO | UZ | 8 | |
Sirena | 12 |
Ďalej umožňuje
- kalibrovať strednú polohu servomotorov (pomocou klávesu H)
- pomocou + a - nájdeme strednú polohu serva
- enter potvrdí hodnotu a pokračuje sa ďalším servom
- kalibrácia sa po vypnutí nezapamätá, pokiaľ ju neuložíte do EEPROM (pozri nižšie)
- kalibrovať limity pre všetky stupne voľnosti (pomocou klávesu J), zobrazenie celej kalibrácie: kláves G
- celú kalibráciu je možné zapísať do trvalej pamäte EEPROM pomocou klávesu E (používať opatrne! - dá sa prepísať najviac 100000-krát), po zapnutí sa automaticky načíta
- pozdraví po klávese -, ruky dá hore na kláves R
- načíta choreografiu po klávese @,
- zobrazí načítanú choreografiu na kláves ? a
- zatancuje načítanú choreografiu - kláves t.
- Na klávesách 3-6 je možné nastaviť rôznu kombináciu (funkcie kombinacia1() - kombinacia4()).
- Kláves U testuje ultrazvuk
- Kláves S zobrazí aktuálnu polohu servomotorov
- Medzera resetne všetky servo motory do strednej polohy.
Pozrite si (a skopírujte si) príklady choreografií: Otto - príklady choreografií
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 #define FIS3 2960
81 #define G3 3136
82
83 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 };
84 int16_t music_speed;
85
86 uint16_t dlzka_popcorn = 390;
87 uint8_t popcorn[] = { 149, 49,
88 28, 31, 35, 40, 49, 99, 38, 49, 99, 40, 49, 99, 35, 49, 99, 31, 49, 99, 35, 49, 99, 28, 49, 99, 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, 149,
90 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,
91 28, 31, 35, 40, 49, 99, 38, 49, 99, 40, 49, 99, 35, 49, 99, 31, 49, 99, 35, 49, 99, 28, 49, 99, 49,
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, 149,
93 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,
94 49, 35, 38, 43, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 38, 49, 99, 43, 49, 99, 35, 49, 99,
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, 149 ,
96 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,
97 49, 35, 38, 43, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 38, 49, 99, 43, 49, 99, 35, 49, 99,
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, 149 ,
99 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
100 };
101
102 void setup() {
103 Serial.begin(9600);
104 init_serial(9600);
105 init_ultrasonic();
106
107 melodia = popcorn;
108 dlzka_melodie = dlzka_popcorn;
109
110 randomSeed(analogRead(1));
111 s[0].attach(PIN_SERVO_PRAVA_PATA);
112 s[1].attach(PIN_SERVO_LAVA_PATA);
113 s[2].attach(PIN_SERVO_PRAVA_NOHA);
114 s[3].attach(PIN_SERVO_LAVA_NOHA);
115 s[4].attach(PIN_SERVO_LAVA_RUKA);
116 s[5].attach(PIN_SERVO_PRAVA_RUKA);
117 precitaj_kalibraciu_z_EEPROM();
118 for (int i = 0; i < 6; i++)
119 {
120 kalib[i] = prednastavena_kalibracia[i];
121 stav[i] = kalib[i];
122 s[i].write(stav[i]);
123 }
124 ch_len = 0;
125 krok = 7;
126 spomalenie = 6;
127 ahoj();
128 ruky2();
129 delay(100);
130 serial_println("\r\n Otto DTDT");
131 }
132
133 void loop() {
134 char z = -1;
135 if (serial_available()) z = serial_read();
136 #ifdef ECHO_BT_TO_USB
137 if (Serial.available()) z = Serial.read();
138 #endif
139
140 if (z != -1)
141 {
142 if (pohyb_znakom(z)) return;
143 else if (pohyb_kombinacia(z)) return;
144 else if (z == '@') nacitaj_choreografiu();
145 else if (z == '?') vypis_choreografiu();
146 else if (z == 't') zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
147 else if (z == '-') ahoj();
148 else if (z == ' ') reset();
149 else if (z == 'H') kalibruj();
150 else if (z == 'J') nastav_limity();
151 else if (z == 'G') vypis_kalibraciu();
152 else if (z == 'L') nacitaj_kalibraciu();
153 else if (z == 'E') zapis_kalibraciu_do_EEPROM();
154 else if (z == 'R') ruky();
155 else if (z == '9') zvys_krok();
156 else if (z == '1') zniz_krok();
157 else if (z == '8') zvys_spomalenie();
158 else if (z == '7') zniz_spomalenie();
159 else if (z == 'U') test_ultrazvuk();
160 else if (z == 'S') zobraz_stav();
161 }
162 int16_t d = measure_distance();
163 if (d < 10) menu_ultrasonic_request();
164 }
165
166 void otto_translate_tone(uint8_t *note, uint16_t *freq, uint16_t *del)
167 {
168 if (*note == 251)
169 {
170 *freq = ((*(note + 1)) << 8) + (*(note + 2));
171 uint8_t d1 = *(note + 3);
172 uint8_t d2 = *(note + 4);
173 *del = (music_speed * 16 * (long)d1) / d2;
174 }
175 else if (*note == 254)
176 {
177 *freq = FIS3;
178 *del = music_speed;
179 }
180 else if (*note == 255)
181 {
182 *freq = G3;
183 *del = music_speed;
184 }
185 else
186 {
187 uint8_t len = *note / 50;
188 *del = music_speed;
189 while (len--) *del *= 2;
190 uint8_t n = *note % 50;
191 if (n != 49)
192 {
193 uint8_t octave = (n + 5) / 12;
194 n = (n + 5 ) % 12;
195 float ffreq = octave_4[n];
196 octave = 4 - octave;
197 while (octave > 0)
198 {
199 ffreq /= 2.0;
200 octave--;
201 }
202 *freq = (uint16_t)ffreq;
203 }
204 else *freq = 0;
205 }
206 }
207
208 void otto_tone(uint8_t *note)
209 {
210 uint16_t freq, d;
211 otto_translate_tone(note, &freq, &d);
212 if (freq) tone(SIRENA, freq); else noTone(SIRENA);
213 delay(d);
214 noTone(SIRENA);
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 void test_ultrazvuk()
233 {
234 int i = 0;
235 while ((serial_available() == 0) && (Serial.available() == 0))
236 {
237 serial_println_num(measure_distance());
238 delay(100);
239 }
240 serial_read();
241 }
242
243 void menu_ultrasonic_request()
244 {
245 uint32_t tm = millis();
246 int d = measure_distance();
247 int count = 0;
248 int count2 = 0;
249 while ((millis() - tm < 1500) && ((d < 15) || (count2 < 5)) && (count < 10))
250 {
251 delay(10);
252 d = measure_distance();
253 if (d == 10000) count++;
254 else count = 0;
255 if (d >= 15) count2++;
256 else count2 = 0;
257 if (serial_available() || Serial.available()) return;
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 == 1) vpred();
301 if (cmd == 2) zahraj_melodiu();
302 if (cmd == 3) melodia_jedna_druhej();
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 // chodza pre vacsieho dreveneho robota
331 uint16_t chor_time[] = {100,1,1,100,1,1,1,100,1,1,1,100,1,100,1,0};
332 uint8_t chor_servo[] = {1,2,6,4,3,6,5,1,2,6,5,3,4,1,2,0};
333 uint8_t chor_val[] = {48,69,180,104,104,90,0,111,146,0,90,62,69,48,69,0};
334 uint8_t chor_len = 16;
335
336 void vpred()
337 {
338 while (measure_distance() > 30)
339 zatancuj_choreografiu(chor_time, chor_servo, chor_val, chor_len);
340 pipni();
341 reset();
342 }
343
344 void precitaj_kalibraciu_z_EEPROM()
345 {
346 uint8_t value = EEPROM.read(1);
347 if (value != '~') return;
348 for (int i = 2; i < 8; i++)
349 prednastavena_kalibracia[i - 2] = EEPROM.read(i);
350 for (int i = 0; i < 6; i++)
351 dolny_limit[i] = EEPROM.read(i + 9);
352 for (int i = 0; i < 6; i++)
353 horny_limit[i] = EEPROM.read(i + 15);
354 }
355
356 char read_char()
357 {
358 while (!serial_available() && !Serial.available());
359 if (serial_available()) return serial_read();
360 else return Serial.read();
361 }
362
363 void zapis_kalibraciu_do_EEPROM()
364 {
365 serial_print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
366 char odpoved = read_char();
367 serial_println_char(odpoved);
368 if (odpoved == 'Y')
369 {
370 EEPROM.write(1, '~');
371 for (int i = 2; i < 8; i++)
372 EEPROM.write(i, kalib[i - 2]);
373 for (int i = 0; i < 6; i++)
374 EEPROM.write(9 + i, dolny_limit[i]);
375 for (int i = 0; i < 6; i++)
376 EEPROM.write(15 + i, horny_limit[i]);
377 serial_println("ok");
378 }
379 }
380
381 void pipni()
382 {
383 tone(SIRENA, 1568, 50);
384 delay(100);
385 tone(SIRENA, 1357, 50);
386 }
387
388 void ruky()
389 {
390 int odloz_krok = krok;
391 delay(500);
392 krok = 90;
393 pohyb(SERVO_LAVA_RUKA);
394 pohyb(SERVO_PRAVA_RUKA);
395 delay(1000);
396 krok = 180;
397 pohyb(-SERVO_LAVA_RUKA);
398 pohyb(-SERVO_PRAVA_RUKA);
399 delay(1000);
400 krok = odloz_krok;
401 pipni();
402 }
403
404 void ruky2()
405 {
406 int odloz_krok = krok;
407 delay(500);
408 krok = 180;
409 pohyb(SERVO_LAVA_RUKA);
410 pohyb(SERVO_PRAVA_RUKA);
411 delay(1000);
412 krok = 90;
413 pohyb(-SERVO_LAVA_RUKA);
414 pohyb(-SERVO_PRAVA_RUKA);
415 delay(1000);
416 krok = odloz_krok;
417 pipni();
418 }
419
420 void ahoj()
421 {
422 tone(SIRENA, 1568, 50);
423 delay(70);
424 tone(SIRENA, 1175, 30);
425 delay(50);
426 tone(SIRENA, 880, 30);
427 delay(50);
428 tone(SIRENA, 1047, 50);
429 delay(70);
430 tone(SIRENA, 1245, 30);
431 delay(150);
432 tone(SIRENA, 1568, 50);
433 delay(100);
434 if (random(10) > 4) tone(SIRENA, 1357, 50);
435 else tone(SIRENA, 1047, 50);
436 }
437
438 void nastav_koncatinu(int8_t servo, uint8_t poloha)
439 {
440 int8_t delta;
441 int8_t srv = (servo > 0)?servo:-servo;
442 srv--;
443 poloha += kalib[srv] - 90;
444 if (poloha > 180) poloha = 180;
445 if (poloha < 0) poloha = 0;
446
447 if (stav[srv] < poloha) delta = 1;
448 else delta = -1;
449 while (stav[srv] != poloha)
450 {
451 stav[srv] +=delta;
452 s[srv].write(stav[srv]);
453 delay(spomalenie);
454 }
455 }
456
457 void zobraz_stav()
458 {
459 for (int i = 0; i < 6; i++)
460 {
461 serial_print("S"); serial_print_num(i + 1); serial_print(": "); serial_println_num(stav[i] - kalib[i] + 90);
462 }
463 serial_println("---");
464 pipni();
465 }
466
467 void pohyb(int8_t servo)
468 {
469 int8_t srv = (servo > 0)?servo:-servo;
470 srv--;
471 if (servo_invertovane[srv]) servo = -servo;
472 if (servo > 0)
473 {
474 if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
475 else stav[srv] = horny_limit[srv];
476 s[srv].write(stav[srv]);
477 }
478 else if (servo < 0)
479 {
480 if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok;
481 else stav[srv] = dolny_limit[srv];
482 s[srv].write(stav[srv]);
483 }
484 }
485
486 uint8_t pohyb_znakom(char z)
487 {
488 for (int i = 0; i < 12; i++)
489 {
490 if (z == znaky_zmien[i])
491 {
492 int8_t servo = zmeny[i];
493 pohyb(servo);
494 }
495 }
496 }
497
498 void kombinacia1()
499 {
500 pohyb(SERVO_LAVA_NOHA);
501 pohyb(-SERVO_PRAVA_PATA);
502 }
503
504 void kombinacia2()
505 {
506 pohyb(SERVO_PRAVA_NOHA);
507 pohyb(-SERVO_LAVA_PATA);
508 }
509
510 void kombinacia3()
511 {
512 pohyb(SERVO_LAVA_RUKA);
513 pohyb(SERVO_PRAVA_RUKA);
514 }
515
516 void kombinacia4()
517 {
518 pohyb(-SERVO_LAVA_RUKA);
519 pohyb(-SERVO_PRAVA_RUKA);
520 }
521
522 int pohyb_kombinacia(char z)
523 {
524 if (z == '3') kombinacia1();
525 else if (z == '4') kombinacia2();
526 else if (z == '5') kombinacia3();
527 else if (z == '6') kombinacia4();
528 else return 0;
529 return 1;
530 }
531
532 int nacitajCislo()
533 {
534 int num = 0;
535 int z;
536 do {
537 z = read_char();
538 if (z == '#') while (z != 13) z = read_char();
539 } while ((z < '0') || (z > '9'));
540 while ((z >= '0') && (z <= '9'))
541 {
542 num *= 10;
543 num += (z - '0');
544 do { z = read_char(); if (z == -1) delayMicroseconds(10); } while (z < 0);
545 }
546 return num;
547 }
548
549 void nacitaj_choreografiu()
550 {
551 ch_len = 0;
552 int tm;
553 do {
554 tm = nacitajCislo();
555 ch_time[ch_len] = tm;
556 ch_servo[ch_len] = nacitajCislo();
557 ch_val[ch_len] = nacitajCislo();
558 ch_len++;
559 if (ch_len == CHOREO_LEN) break;
560 } while (tm > 0);
561 pipni();
562 }
563
564 void vypis_choreografiu()
565 {
566 for (int i = 0; i < ch_len; i++)
567 {
568 serial_print_num(ch_time[i]);
569 serial_print(" ");
570 serial_print_num(ch_servo[i]);
571 serial_print(" ");
572 serial_println_num(ch_val[i]);
573 }
574 pipni();
575 }
576
577 void zatancuj_choreografiu(uint16_t *ch_time, uint8_t *ch_servo, uint8_t *ch_val, int ch_len )
578 {
579 for (int i = 0; i < ch_len - 1; i++)
580 {
581 delay(ch_time[i]);
582 nastav_koncatinu(ch_servo[i], ch_val[i]);
583 }
584 pipni();
585 while (serial_available()) serial_read();
586 while (Serial.available()) Serial.read();
587 }
588
589 void reset()
590 {
591 for (int i = 0; i < 6; i++)
592 {
593 stav[i] = kalib[i];
594 s[i].write(kalib[i]);
595 delay(300);
596 }
597 pipni();
598 }
599
600 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
601 {
602 serial_print(" (+/-/ENTER): ");
603 serial_println_num(hodnota);
604 s[servo].write(hodnota);
605 char z;
606 do {
607 z = read_char();
608 if ((z == '+') && (hodnota < 180)) hodnota++;
609 else if ((z == '-') && (hodnota > 0)) hodnota--;
610 if ((z == '+') || (z == '-'))
611 {
612 serial_print_num(hodnota); serial_print_char('\r');
613 s[servo].write(hodnota);
614 }
615 } while (z != 13);
616 return hodnota;
617 }
618
619 void kalibruj()
620 {
621 for (int i = 0; i < 6; i++)
622 {
623 serial_print_num(i);
624 kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
625 serial_print_num(i);
626 serial_print(": ");
627 serial_println_num(kalib[i]);
628 }
629 for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
630 serial_println("ok");
631 pipni();
632 }
633
634 void nastav_limity()
635 {
636 for (int i = 0; i < 6; i++)
637 {
638 serial_print_num(i);
639 serial_print("dolny");
640 dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
641 serial_print_num(i);
642 serial_print(" dolny: ");
643 serial_println_num(dolny_limit[i]);
644 s[i].write(kalib[i]);
645
646 serial_print_num(i);
647 serial_print("horny");
648 horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
649 serial_print_num(i);
650 serial_print(" horny: ");
651 serial_println_num(horny_limit[i]);
652 s[i].write(kalib[i]);
653 }
654 for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print("-"); serial_print_num(horny_limit[i]); serial_print(" "); }
655 serial_println("ok");
656 pipni();
657 }
658
659 void vypis_kalibraciu()
660 {
661 serial_print("stredy: ");
662 for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
663 serial_println();
664 serial_print("dolny limit: ");
665 for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print(" "); }
666 serial_println();
667 serial_print("horny limit: ");
668 for (int i = 0; i < 6; i++) { serial_print_num(horny_limit[i]); serial_print(" "); }
669 serial_println();
670 }
671
672 void nacitaj_kalibraciu()
673 {
674 int tm;
675 for (int i = 0; i < 6; i++)
676 kalib[i] = nacitajCislo();
677 vypis_kalibraciu();
678 serial_println("ok");
679 pipni();
680 }
681
682 void zvys_krok()
683 {
684 if (krok < 180) krok++;
685 serial_print("krok: ");
686 serial_println_num(krok);
687 }
688
689 void zniz_krok()
690 {
691 if (krok > 0) krok--;
692 serial_print("krok: ");
693 serial_println_num(krok);
694 }
695
696 void zniz_spomalenie()
697 {
698 if (spomalenie > 0) spomalenie--;
699 serial_print("spomalenie: ");
700 serial_println_num(spomalenie);
701 }
702
703 void zvys_spomalenie()
704 {
705 if (spomalenie < 100) spomalenie++;
706 serial_print("spomalenie: ");
707 serial_println_num(spomalenie);
708 }
709
710 // nasleduje softverova implementacia serioveho portu
711 #define SERIAL_STATE_IDLE 0
712 #define SERIAL_STATE_RECEIVING 1
713 #define SERIAL_BUFFER_LENGTH 20
714
715 static volatile uint8_t serial_state;
716 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
717 static volatile uint8_t serial_buf_wp, serial_buf_rp;
718
719 static volatile uint8_t receiving_byte;
720
721 static volatile uint32_t time_startbit_noticed;
722 static volatile uint8_t next_bit_order;
723 static volatile uint8_t waiting_stop_bit;
724 static uint16_t one_byte_duration;
725 static uint16_t one_bit_duration;
726 static uint16_t one_bit_write_duration;
727 static uint16_t half_of_one_bit_duration;
728
729 void init_serial(uint32_t baud_rate)
730 {
731 pinMode(2, INPUT);
732 pinMode(4, OUTPUT);
733
734 serial_state = SERIAL_STATE_IDLE;
735
736 one_byte_duration = 9500000 / baud_rate;
737 one_bit_duration = 1000000 / baud_rate;
738 one_bit_write_duration = one_bit_duration - 1;
739 half_of_one_bit_duration = 500000 / baud_rate;
740
741 PCMSK2 |= 4; //PCINT18;
742 PCIFR &= ~4; //PCIF2;
743 PCICR |= 4; // PCIE2;
744 }
745
746 ISR(PCINT2_vect)
747 {
748 uint32_t tm = micros();
749 if (serial_state == SERIAL_STATE_IDLE)
750 {
751 time_startbit_noticed = tm;
752 serial_state = SERIAL_STATE_RECEIVING;
753 receiving_byte = 0xFF;
754 next_bit_order = 0;
755 }
756 else if (tm - time_startbit_noticed > one_byte_duration)
757 {
758 serial_buffer[serial_buf_wp] = receiving_byte;
759 serial_buf_wp++;
760 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
761 time_startbit_noticed = tm;
762 receiving_byte = 0xFF;
763 next_bit_order = 0;
764 }
765 else if (PIND & 4)
766 {
767 int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
768 while (next_bit_order < new_next_bit_order)
769 {
770 receiving_byte &= ~(1 << next_bit_order);
771 next_bit_order++;
772 }
773 if (next_bit_order == 8)
774 {
775 serial_buffer[serial_buf_wp] = receiving_byte;
776 serial_buf_wp++;
777 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
778 serial_state = SERIAL_STATE_IDLE;
779 }
780 } else
781 next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
782 }
783
784 uint8_t serial_available()
785 {
786 cli();
787 if (serial_buf_rp != serial_buf_wp)
788 {
789 sei();
790 return 1;
791 }
792 if (serial_state == SERIAL_STATE_RECEIVING)
793 {
794 uint32_t tm = micros();
795 if (tm - time_startbit_noticed > one_byte_duration)
796 {
797 serial_state = SERIAL_STATE_IDLE;
798 serial_buffer[serial_buf_wp] = receiving_byte;
799 serial_buf_wp++;
800 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
801 sei();
802 return 1;
803 }
804 }
805 sei();
806 return 0;
807 }
808
809 int16_t serial_read()
810 {
811 cli();
812 if (serial_buf_rp != serial_buf_wp)
813 {
814 uint8_t ch = serial_buffer[serial_buf_rp];
815 serial_buf_rp++;
816 if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
817 sei();
818 return ch;
819 }
820
821 if (serial_state == SERIAL_STATE_RECEIVING)
822 {
823 uint32_t tm = micros();
824 if (tm - time_startbit_noticed > one_byte_duration)
825 {
826 uint8_t ch = receiving_byte;
827 serial_state = SERIAL_STATE_IDLE;
828 sei();
829 return ch;
830 }
831 }
832 sei();
833 return -1;
834 }
835
836 void serial_write(uint8_t ch)
837 {
838 #ifdef ECHO_BT_TO_USB
839 Serial.print((char)ch);
840 #endif
841 PORTD &= ~16;
842 delayMicroseconds(one_bit_write_duration);
843 for (uint8_t i = 0; i < 8; i++)
844 {
845 if (ch & 1) PORTD |= 16;
846 else PORTD &= ~16;
847 ch >>= 1;
848 delayMicroseconds(one_bit_write_duration);
849 }
850 PORTD |= 16;
851 delayMicroseconds(one_bit_write_duration);
852 delayMicroseconds(one_bit_write_duration);
853 delayMicroseconds(one_bit_write_duration);
854 delayMicroseconds(one_bit_write_duration);
855 delayMicroseconds(one_bit_write_duration);
856 }
857
858 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
859 {
860 uint16_t len;
861 uint16_t ch;
862 do {
863 ch = serial_read();
864 if (ch == 13) continue;
865 } while (ch == -1);
866
867 do {
868 if ((ch != 13) && (ch != 10) && (ch != -1))
869 {
870 *(ln++) = ch;
871 max_length--;
872 len++;
873 }
874 ch = serial_read();
875 } while ((ch != 13) && max_length);
876 *ln = 0;
877 return len;
878 }
879
880 void serial_print_num(int32_t number)
881 {
882 if (number < 0)
883 {
884 serial_write('-');
885 number = -number;
886 }
887 int32_t rad = 1;
888 while (number / rad) rad *= 10;
889 if (number > 0) rad /= 10;
890 while (rad)
891 {
892 serial_write((char)('0' + (number / rad)));
893 number -= (number / rad) * rad;
894 rad /= 10;
895 }
896 }
897
898 void serial_print_char(char ch)
899 {
900 serial_write(ch);
901 }
902
903 void serial_print(const uint8_t *str)
904 {
905 while (*str) serial_write(*(str++));
906 }
907
908 void serial_println(const uint8_t *str)
909 {
910 serial_print(str);
911 serial_write(13);
912 serial_write(10);
913 }
914
915 void serial_println_num(int32_t number)
916 {
917 serial_print_num(number);
918 serial_println();
919 }
920
921 void serial_println_char(char ch)
922 {
923 serial_write(ch);
924 serial_println();
925 }
926
927 void serial_println()
928 {
929 serial_write(13);
930 serial_write(10);
931 }
932
933 // nasleduje citanie z utltazvukoveho senzora
934
935 static volatile uint32_t pulse_start;
936 static volatile uint8_t new_distance;
937
938 void init_ultrasonic()
939 {
940 pinMode(US_ECHO, INPUT);
941 pinMode(US_TRIG, OUTPUT);
942
943 PCMSK0 |= 1; //PCINT0;
944 PCIFR &= ~1; //PCIF0;
945 PCICR |= 1; // PCIE0;
946 }
947
948 ISR(PCINT0_vect)
949 {
950 if (PINB & 1) pulse_start = micros();
951 else
952 {
953 distance = (int16_t)((micros() - pulse_start) / 58);
954 new_distance = 1;
955 }
956 }
957
958 void start_distance_measurement()
959 {
960 distance = 10000;
961 new_distance = 0;
962 digitalWrite(US_TRIG, HIGH);
963 delayMicroseconds(10);
964 digitalWrite(US_TRIG, LOW);
965 }
966
967 void wait_for_distance_measurement_to_complete()
968 {
969 uint8_t counter = 0;
970 while ((counter < 20) && !new_distance)
971 {
972 delay(1);
973 counter++;
974 }
975 if (counter == 20)
976 {
977 pinMode(US_ECHO, OUTPUT);
978 digitalWrite(US_ECHO, HIGH);
979 delayMicroseconds(10);
980 digitalWrite(US_ECHO, LOW);
981 pinMode(US_ECHO, INPUT);
982 delayMicroseconds(5);
983 distance = 10000;
984 }
985 }
986
987 int16_t measure_distance()
988 {
989 start_distance_measurement();
990 wait_for_distance_measurement_to_complete();
991 return distance;
992 }