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

From DT^2
Jump to: navigation, search
Line 243: Line 243:
 
void menu_command(int cmd)
 
void menu_command(int cmd)
 
{
 
{
 +
  if (cmd == 2) ahoj();
 
   serial_println_num(cmd);
 
   serial_println_num(cmd);
 
}
 
}
Line 421: Line 422:
 
   int z;
 
   int z;
 
   do {
 
   do {
     z = serial_read();
+
     z = read_char();
     if (z == '#') while (z != 13) z = serial_read();
+
     if (z == '#') while (z != 13) z = read_char();
 
   } while ((z < '0') || (z > '9'));
 
   } while ((z < '0') || (z > '9'));
 
   while ((z >= '0') && (z <= '9'))
 
   while ((z >= '0') && (z <= '9'))
Line 428: Line 429:
 
     num *= 10;
 
     num *= 10;
 
     num += (z - '0');
 
     num += (z - '0');
     do { z = serial_read(); if (z == -1) delayMicroseconds(10); } while (z < 0);
+
     do { z = read_char(); if (z == -1) delayMicroseconds(10); } while (z < 0);
 
   }
 
   }
 
   return num;
 
   return num;

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