Difference between revisions of "Otto - riadiaci program v.2"

From DT^2
Jump to: navigation, search
Line 5: Line 5:
  
 
{|class="wikitable"
 
{|class="wikitable"
!klaves!!servo!!pin
+
!klaves!!servo!!pin!!Kalibracia
 
|-
 
|-
|a/q || ľavá ruka ||  10
+
|a/q || ľavá ruka ||  10 ||4
 
|-
 
|-
|;/p || pravá ruka||  11
+
|;/p || pravá ruka||  11 ||5
 
|-
 
|-
|z/x || ľavá noha  || 9
+
|z/x || ľavá noha  || 9 ||3
 
|-
 
|-
|,/. || pravá noha || 6
+
|,/. || pravá noha || 6 ||2
 
|-
 
|-
|d/c || ľavá päta  || 5
+
|d/c || ľavá päta  || 5 ||1
 
|-
 
|-
|k/m || pravá päta || 3
+
|k/m || pravá päta || 3 ||0
 
|-
 
|-
 
|1/9 || zníž/zvýš rýchlosť pohybu || -
 
|1/9 || zníž/zvýš rýchlosť pohybu || -

Revision as of 10:57, 3 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 Kalibracia
a/q ľavá ruka 10 4
;/p pravá ruka 11 5
z/x ľavá noha 9 3
,/. pravá noha 6 2
d/c ľavá päta 5 1
k/m pravá päta 3 0
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 - najdeme nulovu polohu serva
    • eneter ideme na dalsie servo
    • ukoncime E ulozenie do eprom
    • potvrdime Y
  • 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!), 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
  • Medzera resetne všetky servo motory do strednej polohy.
  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 
 74 void setup() {
 75   Serial.begin(9600);
 76   init_serial(9600);
 77   init_ultrasonic();
 78 
 79   randomSeed(analogRead(1));
 80   s[0].attach(PIN_SERVO_PRAVA_PATA);
 81   s[1].attach(PIN_SERVO_LAVA_PATA);
 82   s[2].attach(PIN_SERVO_PRAVA_NOHA);
 83   s[3].attach(PIN_SERVO_LAVA_NOHA);
 84   s[4].attach(PIN_SERVO_PRAVA_RUKA);
 85   s[5].attach(PIN_SERVO_LAVA_RUKA);
 86   precitaj_kalibraciu_z_EEPROM();
 87   for (int i = 0; i < 6; i++)
 88   {
 89     kalib[i] = prednastavena_kalibracia[i];
 90     stav[i] = kalib[i];
 91     s[i].write(stav[i]);
 92   }
 93   ch_len = 0;
 94   krok = 7;
 95   ahoj();
 96   ruky2();
 97   delay(100);  
 98   serial_println("\r\n  Otto DTDT");
 99 }
100 
101 void loop() {
102   char z = -1;
103   if (serial_available()) z = serial_read();
104 #ifdef ECHO_BT_TO_USB
105     if (Serial.available()) z = Serial.read();
106 #endif  
107   
108   if (z != -1)
109   { 
110     if (pohyb_znakom(z)) return;   
111     else if (pohyb_kombinacia(z)) return; 
112     else if (z == '@') nacitaj_choreografiu();
113     else if (z == '?') vypis_choreografiu();
114     else if (z == 't') zatancuj_choreografiu();
115     else if (z == '-') ahoj();
116     else if (z == ' ') reset();
117     else if (z == 'H') kalibruj();
118     else if (z == 'J') nastav_limity();
119     else if (z == 'G') vypis_kalibraciu();
120     else if (z == 'L') nacitaj_kalibraciu();
121     else if (z == 'E') zapis_kalibraciu_do_EEPROM();
122     else if (z == 'R') ruky();
123     else if (z == '9') zvys_krok();
124     else if (z == '1') zniz_krok();
125     else if (z == 'U') test_ultrazvuk();
126   }
127   int16_t d = measure_distance();
128   if (d < 10) menu_ultrasonic_request();
129 }
130 
131 void test_ultrazvuk()
132 {
133   int i = 0;
134   while ((serial_available() == 0) && (Serial.available() == 0))
135   {
136     serial_println_num(measure_distance());
137     delay(100);
138   }
139   serial_read();
140 }
141 
142 void menu_ultrasonic_request()
143 {
144   uint32_t tm = millis();
145   int d = measure_distance();
146   while ((millis() - tm < 3000) && (d < 15)) 
147   {
148     delay(10);
149     d = measure_distance();
150   }
151   if (d < 15)
152     ultrasonic_menu();
153 }
154 
155 void ultrasonic_menu()
156 {
157   int selection = 0;
158   tone(SIRENA, 880, 200);
159   
160   do {  
161     int count = 0;
162     do { 
163       int32_t d = measure_distance();
164       if (d == 10000) continue;
165       if (d >= 20) count++;
166       else count = 0;
167       delay(10);
168     } while (!serial_available() && !Serial.available() && (count < 20));
169     
170     tone(SIRENA, 440, 200);
171     uint32_t tm = millis();
172     while ((measure_distance() > 15) && (millis() - tm < 1500) && !serial_available() && !Serial.available()) delay(10);
173     if (millis() - tm >= 1500) 
174     {
175       tone(SIRENA, 2000, 50);
176       menu_command(selection);
177       return;
178     }       
179     selection++;
180     for (int i = 0; i < selection; i++)
181     {
182       tone(SIRENA, 1261, 50);
183       delay(250);
184     }
185   } while (!serial_available() && !Serial.available());
186   while (serial_available()) serial_read();
187   while (Serial.available()) Serial.read();
188 }
189 
190 void menu_command(int cmd)
191 {
192   serial_println_num(cmd);
193 }
194 
195 void precitaj_kalibraciu_z_EEPROM()
196 {
197   uint8_t value = EEPROM.read(1);
198   if (value != '~') return;
199   for (int i = 2; i < 8; i++)
200     prednastavena_kalibracia[i - 2] = EEPROM.read(i);
201   for (int i = 0; i < 6; i++)
202     dolny_limit[i] = EEPROM.read(i + 9);
203   for (int i = 0; i < 6; i++)
204     horny_limit[i] = EEPROM.read(i + 15);    
205 }
206 
207 char read_char()
208 {
209   while (!serial_available() && !Serial.available());
210   if (serial_available()) return serial_read();
211   else return Serial.read();
212 }
213 
214 void zapis_kalibraciu_do_EEPROM()
215 {
216   serial_print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
217   char odpoved = read_char();
218   serial_println_char(odpoved);
219   if (odpoved == 'Y') 
220   {
221     EEPROM.write(1, '~');
222     for (int i = 2; i < 8; i++)
223       EEPROM.write(i, kalib[i - 2]);
224     for (int i = 0; i < 6; i++)
225       EEPROM.write(9 + i, dolny_limit[i]);
226     for (int i = 0; i < 6; i++)
227       EEPROM.write(15 + i, horny_limit[i]);
228     serial_println("ok");
229   }
230 }
231 
232 void pipni()
233 {
234   tone(SIRENA, 1568, 50);
235   delay(100);
236   tone(SIRENA, 1357, 50);
237 }
238 
239 void ruky()
240 {
241   int odloz_krok = krok;
242   delay(500);
243   krok = 90;
244   pohyb(SERVO_LAVA_RUKA);    
245   pohyb(SERVO_PRAVA_RUKA);
246   delay(1000);
247   krok = 180;
248   pohyb(-SERVO_LAVA_RUKA);    
249   pohyb(-SERVO_PRAVA_RUKA);
250   delay(1000);
251   krok = odloz_krok;
252   pipni();
253 }
254 
255 void ruky2()
256 {
257   int odloz_krok = krok;
258   delay(500);
259   krok = 180;
260   pohyb(SERVO_LAVA_RUKA);    
261   pohyb(SERVO_PRAVA_RUKA);
262   delay(1000);
263   krok = 90;
264   pohyb(-SERVO_LAVA_RUKA);    
265   pohyb(-SERVO_PRAVA_RUKA);
266   delay(1000);
267   krok = odloz_krok;
268   pipni();
269 }
270 
271 void ahoj()
272 {
273   tone(SIRENA, 1568, 50);
274   delay(70);
275   tone(SIRENA, 1175, 30);
276   delay(50);
277   tone(SIRENA, 880, 30);
278   delay(50);
279   tone(SIRENA, 1047, 50);
280   delay(70);
281   tone(SIRENA, 1245, 30);
282   delay(150);
283   tone(SIRENA, 1568, 50);
284   delay(100);
285   if (random(10) > 4) tone(SIRENA, 1357, 50);
286   else tone(SIRENA, 1047, 50);
287 }
288 
289 void nastav_koncatinu(int8_t servo, uint8_t poloha)
290 {
291   int8_t srv = (servo > 0)?servo:-servo;
292   srv--;
293   poloha += kalib[srv] - 90;
294   if (poloha > 180) poloha = 180;
295   if (poloha < 0) poloha = 0;
296   stav[srv] = poloha;
297   s[srv].write(stav[srv]);
298 }
299 
300 void pohyb(int8_t servo)
301 {
302   int8_t srv = (servo > 0)?servo:-servo;
303   srv--;
304   if (servo_invertovane[srv]) servo = -servo;
305   if (servo > 0)
306   {
307     if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
308     else stav[srv] = horny_limit[srv];
309     s[srv].write(stav[srv]);
310   }
311   else if (servo < 0)
312   {
313     if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok; 
314     else stav[srv] = dolny_limit[srv];
315     s[srv].write(stav[srv]);      
316   }
317 }
318 
319 uint8_t pohyb_znakom(char z)
320 {
321   for (int i = 0; i < 12; i++)
322   {
323     if (z == znaky_zmien[i])
324     {
325       int8_t servo = zmeny[i];
326       pohyb(servo);
327     }
328   }
329 }
330 
331 void kombinacia1()
332 {
333   pohyb(SERVO_LAVA_NOHA);
334   pohyb(-SERVO_PRAVA_PATA);
335 }
336 
337 void kombinacia2()
338 {
339   pohyb(SERVO_PRAVA_NOHA);
340   pohyb(-SERVO_LAVA_PATA);
341 }
342 
343 void kombinacia3()
344 {
345    pohyb(SERVO_LAVA_RUKA); 
346    pohyb(SERVO_PRAVA_RUKA); 
347 }
348 
349 void kombinacia4()
350 {
351    pohyb(-SERVO_LAVA_RUKA); 
352    pohyb(-SERVO_PRAVA_RUKA); 
353 }
354 
355 int pohyb_kombinacia(char z)
356 {
357   if (z == '3') kombinacia1();
358   else if (z == '4') kombinacia2();
359   else if (z == '5') kombinacia3();
360   else if (z == '6') kombinacia4();
361   else return 0;
362   return 1;
363 }
364 
365 int nacitajCislo()
366 {
367   int num = 0;
368   int z;
369   do {
370     z = serial_read();
371     if (z == '#') while (z != 13) z = serial_read();
372   } while ((z < '0') || (z > '9'));
373   while ((z >= '0') && (z <= '9'))
374   {
375     num *= 10;
376     num += (z - '0');
377     do { z = serial_read(); if (z == -1) delayMicroseconds(10); } while (z < 0);
378   }
379   return num;
380 }
381 
382 void nacitaj_choreografiu()
383 {
384   ch_len = 0;
385   int tm;
386   do { 
387     tm = nacitajCislo();
388     ch_time[ch_len] = tm;
389     ch_servo[ch_len] = nacitajCislo();
390     ch_val[ch_len] = nacitajCislo();
391     ch_len++;
392   if (ch_len == CHOREO_LEN) break;
393   } while (tm > 0);
394   pipni();  
395 }
396 
397 void vypis_choreografiu()
398 {
399   for (int i = 0; i < ch_len; i++)
400   {
401     serial_print_num(ch_time[i]);
402     serial_print(" ");
403     serial_print_num(ch_servo[i]);
404     serial_print(" ");
405     serial_println_num(ch_val[i]);      
406   }
407   pipni();
408 }
409 
410 void zatancuj_choreografiu()
411 {
412   for (int i = 0; i < ch_len; i++)
413   {
414     delay(ch_time[i]);
415     nastav_koncatinu(ch_servo[i], ch_val[i]);
416   }
417   if (ch_len > 0) stav[ch_servo[ch_len - 1]] = ch_val[ch_len - 1];
418   pipni();
419 }
420 
421 void reset()
422 {
423   for (int i = 0; i < 6; i++) 
424   {
425     stav[i] = kalib[i];
426     s[i].write(kalib[i]);
427   }
428   pipni();
429 }
430 
431 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
432 {
433     serial_print(" (+/-/ENTER): ");
434     serial_println_num(hodnota);
435     s[servo].write(hodnota);
436     char z;
437     do {
438       z = read_char();
439       if ((z == '+') && (hodnota < 180)) hodnota++;
440       else if ((z == '-') && (hodnota > 0)) hodnota--; 
441       if ((z == '+') || (z == '-'))
442       {
443         serial_print_num(hodnota); serial_print_char('\r'); 
444         s[servo].write(hodnota);
445       }
446     } while (z != 13);
447     return hodnota;
448 }
449 
450 void kalibruj()
451 {
452   for (int i = 0; i < 6; i++)
453   {
454     serial_print_num(i);
455     kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
456     serial_print_num(i);
457     serial_print(": ");
458     serial_println_num(kalib[i]);
459   }
460   for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
461   serial_println("ok");
462   pipni();
463 }
464 
465 void nastav_limity()
466 {
467   for (int i = 0; i < 6; i++)
468   {
469     serial_print_num(i);
470     serial_print("dolny");
471     dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
472     serial_print_num(i);
473     serial_print(" dolny: ");
474     serial_println_num(dolny_limit[i]);
475     s[i].write(kalib[i]);
476 
477     serial_print_num(i);
478     serial_print("horny");
479     horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
480     serial_print_num(i);
481     serial_print(" horny: ");
482     serial_println_num(horny_limit[i]);
483     s[i].write(kalib[i]);
484   }
485   for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print("-"); serial_print_num(horny_limit[i]); serial_print(" "); }
486   serial_println("ok");
487   pipni();
488 }
489 
490 void vypis_kalibraciu()
491 {
492   serial_print("stredy: ");
493   for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
494   serial_println();
495   serial_print("dolny limit: ");
496   for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print(" "); }
497   serial_println();
498   serial_print("horny limit: ");
499   for (int i = 0; i < 6; i++) { serial_print_num(horny_limit[i]); serial_print(" "); }
500   serial_println();
501 }
502 
503 void nacitaj_kalibraciu()
504 {
505   int tm;
506   for (int i = 0; i < 6; i++) 
507     kalib[i] = nacitajCislo();
508   vypis_kalibraciu();
509   serial_println("ok");
510   pipni();
511 }
512 
513 void zvys_krok()
514 {
515   if (krok < 180) krok++;
516   serial_print("krok: ");
517   serial_println_num(krok);
518 }
519 
520 void zniz_krok()
521 {
522   if (krok > 0) krok--;
523   serial_print("krok: ");
524   serial_println_num(krok);
525 }
526 
527 // nasleduje softverova implementacia serioveho portu
528 #define SERIAL_STATE_IDLE      0
529 #define SERIAL_STATE_RECEIVING 1
530 #define SERIAL_BUFFER_LENGTH   20
531 
532 static volatile uint8_t serial_state;
533 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
534 static volatile uint8_t serial_buf_wp, serial_buf_rp;
535 
536 static volatile uint8_t receiving_byte;
537 
538 static volatile uint32_t time_startbit_noticed;
539 static volatile uint8_t next_bit_order;
540 static volatile uint8_t waiting_stop_bit;
541 static uint16_t one_byte_duration;
542 static uint16_t one_bit_duration;
543 static uint16_t one_bit_write_duration;
544 static uint16_t half_of_one_bit_duration;
545 
546 void init_serial(uint32_t baud_rate)
547 {
548   pinMode(2, INPUT);
549   pinMode(4, OUTPUT);
550   
551   serial_state = SERIAL_STATE_IDLE;
552   
553   one_byte_duration = 9500000 / baud_rate;
554   one_bit_duration = 1000000 / baud_rate;
555   one_bit_write_duration = one_bit_duration - 1;
556   half_of_one_bit_duration = 500000 / baud_rate;
557   
558   PCMSK2 |= 4; //PCINT18;
559   PCIFR &= ~4; //PCIF2;
560   PCICR |= 4; // PCIE2;
561 }
562 
563 ISR(PCINT2_vect)
564 {
565   uint32_t tm = micros();
566   if (serial_state == SERIAL_STATE_IDLE)
567   {    
568     time_startbit_noticed = tm;
569     serial_state = SERIAL_STATE_RECEIVING;
570     receiving_byte = 0xFF;
571     next_bit_order = 0;
572   }
573   else if (tm - time_startbit_noticed > one_byte_duration)
574   {
575       serial_buffer[serial_buf_wp] = receiving_byte;
576       serial_buf_wp++;
577       if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
578       time_startbit_noticed = tm;
579       receiving_byte = 0xFF;
580       next_bit_order = 0;
581   }
582   else if (PIND & 4) 
583   {
584      int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
585      while (next_bit_order < new_next_bit_order)
586      {  
587         receiving_byte &= ~(1 << next_bit_order);
588         next_bit_order++;
589      }
590      if (next_bit_order == 8)
591      { 
592         serial_buffer[serial_buf_wp] = receiving_byte;
593         serial_buf_wp++;
594         if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
595         serial_state = SERIAL_STATE_IDLE;
596      }        
597   } else 
598       next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
599 }
600 
601 uint8_t serial_available()
602 {
603   cli();
604   if (serial_buf_rp != serial_buf_wp) 
605   {
606     sei();
607     return 1;
608   }
609   if (serial_state == SERIAL_STATE_RECEIVING)
610   {
611     uint32_t tm = micros();
612     if (tm - time_startbit_noticed > one_byte_duration)
613     {      
614       serial_state = SERIAL_STATE_IDLE;
615       serial_buffer[serial_buf_wp] = receiving_byte;
616       serial_buf_wp++;
617       if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
618       sei();
619       return 1;
620     }
621   }
622   sei();
623   return 0;
624 }
625 
626 int16_t serial_read()
627 {
628   cli();
629   if (serial_buf_rp != serial_buf_wp)
630   {
631     uint8_t ch = serial_buffer[serial_buf_rp];
632     serial_buf_rp++;
633     if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
634     sei();
635     return ch;
636   }
637 
638   if (serial_state == SERIAL_STATE_RECEIVING)
639   {
640     uint32_t tm = micros();
641     if (tm - time_startbit_noticed > one_byte_duration)
642     {
643       uint8_t ch = receiving_byte;
644       serial_state = SERIAL_STATE_IDLE;  
645       sei();
646       return ch;
647     }
648   }
649   sei();
650   return -1;
651 }
652 
653 void serial_write(uint8_t ch)
654 {
655 #ifdef ECHO_BT_TO_USB
656   Serial.print((char)ch);
657 #endif
658   PORTD &= ~16;
659   delayMicroseconds(one_bit_write_duration);
660   for (uint8_t i = 0; i < 8; i++)
661   {
662     if (ch & 1) PORTD |= 16;
663     else PORTD &= ~16;
664     ch >>= 1;
665     delayMicroseconds(one_bit_write_duration);
666   }
667   PORTD |= 16;
668   delayMicroseconds(one_bit_write_duration);
669   delayMicroseconds(one_bit_write_duration);
670   delayMicroseconds(one_bit_write_duration);
671   delayMicroseconds(one_bit_write_duration);
672   delayMicroseconds(one_bit_write_duration);
673 }
674 
675 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
676 {
677   uint16_t len;
678   uint16_t ch;
679   do {
680     ch = serial_read();
681     if (ch == 13) continue;
682   } while (ch == -1);
683 
684   do {
685     if ((ch != 13) && (ch != 10) && (ch != -1)) 
686     {
687       *(ln++) = ch;
688       max_length--;
689       len++;
690     }    
691     ch = serial_read();
692   } while ((ch != 13) && max_length);
693   *ln = 0;
694   return len;
695 }
696 
697 void serial_print_num(int32_t number)
698 {
699   if (number < 0) 
700   {
701     serial_write('-');
702     number = -number;
703   }
704   int32_t rad = 1;
705   while (number / rad) rad *= 10;
706   if (number > 0) rad /= 10;
707   while (rad)
708   {
709     serial_write((char)('0' + (number / rad)));
710     number -= (number / rad) * rad;
711     rad /= 10;
712   }  
713 }
714 
715 void serial_print_char(char ch)
716 {
717   serial_write(ch);  
718 }
719 
720 void serial_print(const uint8_t *str)
721 {
722   while (*str) serial_write(*(str++));
723 }
724 
725 void serial_println(const uint8_t *str)
726 {
727   serial_print(str);
728   serial_write(13);
729   serial_write(10);
730 }
731 
732 void serial_println_num(int32_t number)
733 {
734   serial_print_num(number);
735   serial_println();
736 }
737 
738 void serial_println_char(char ch)
739 {
740   serial_write(ch);
741   serial_println();
742 }
743 
744 void serial_println()
745 {
746   serial_write(13);
747   serial_write(10);
748 }
749 
750 // nasleduje citanie z utltazvukoveho senzora
751 
752 static volatile uint32_t pulse_start;
753 static volatile int16_t distance;
754 static volatile uint8_t new_distance;
755 
756 void init_ultrasonic()
757 {
758   pinMode(US_ECHO, INPUT);
759   pinMode(US_TRIG, OUTPUT);
760   
761   PCMSK0 |= 1; //PCINT0;
762   PCIFR &= ~1; //PCIF0;
763   PCICR |= 1; // PCIE0;  
764 }
765 
766 ISR(PCINT0_vect)
767 {
768   if (PINB & 1) pulse_start = micros();
769   else 
770   {
771     distance = (int16_t)((micros() - pulse_start) / 58);
772     new_distance = 1;
773   }
774 }
775 
776 void start_distance_measurement()
777 {
778   distance = 10000;
779   new_distance = 0;
780   digitalWrite(US_TRIG, HIGH);
781   delayMicroseconds(10);
782   digitalWrite(US_TRIG, LOW);
783 }
784 
785 void wait_for_distance_measurement_to_complete()
786 {
787   uint8_t counter = 0;
788   while ((counter < 20) && !new_distance) 
789   {
790     delay(1);
791     counter++;
792   }
793   if (counter == 20)
794   {
795     pinMode(US_ECHO, OUTPUT);
796     digitalWrite(US_ECHO, HIGH);
797     delayMicroseconds(10);
798     digitalWrite(US_ECHO, LOW);
799     pinMode(US_ECHO, INPUT); 
800     delayMicroseconds(5);
801     distance = 10000;
802   }
803 }
804 
805 int16_t measure_distance()
806 {
807   start_distance_measurement();
808   wait_for_distance_measurement_to_complete();
809   return distance;
810 }