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

From DT^2
Jump to: navigation, search
Line 3: Line 3:
  
 
Na komunikáciu s robotom môžete použiť napr. program Putty: [https://the.earth.li/~sgtatham/putty/latest/w64/putty.exe putty.exe].
 
Na komunikáciu s robotom môžete použiť napr. program Putty: [https://the.earth.li/~sgtatham/putty/latest/w64/putty.exe putty.exe].
 +
 +
Pripojenie na vývody Arduina:
  
 
{|class="wikitable"
 
{|class="wikitable"
!klaves!!servo!!pin!!číslo serva v choreografii
+
!servo!!pin!!
 +
|-
 +
|ľavá ruka ||10
 +
|-
 +
|pravá ruka||11
 +
|-
 +
|ľavá noha||9
 +
|-
 +
|pravá noha||6
 +
|-
 +
|ľavá päta||5
 
|-
 
|-
|a/q || ľavá ruka ||  10 ||5
+
|pravá päta||3
 
|-
 
|-
|;/p || pravá ruka||  11 ||6
+
|TXD BlueTooth||2
 
|-
 
|-
|z/x || ľavá noha  || 9 ||4
+
|RXD BlueTooth||4
 
|-
 
|-
|,/. || pravá noha || 6 ||3
+
|TRIG Ultrazvuk||7
 
|-
 
|-
|d/c || ľavá päta  || 5 ||2
+
|ECHO Ultrazvuk||8
 +
|-
 +
|Sirena||12
 +
|} 
 +
 
 +
Číslovanie serva v choreografii:
 +
 
 +
{|class="wikitable"
 +
!servo!!číslo serva v choreografii
 
|-
 
|-
|k/m || pravá päta || 3 ||1
+
|ľavá ruka || 5
 
|-
 
|-
|1/9 || zníž/zvýš rýchlosť pohybu || -
+
|pravá ruka|| 6
 
|-
 
|-
|TXD || BT|| 2
+
|ľavá noha || 4
 
|-
 
|-
|RXD || BT||4
+
|pravá noha || 3
 
|-
 
|-
|TRIG||UZ ||7
+
|ľavá päta  || 2
 
|-
 
|-
|ECHO||UZ||8
+
|pravá päta || 1
 
|-
 
|-
|Sirena|| ||12
 
 
|}   
 
|}   
  
Ďalej umožňuje
+
{|class="wikitable"
*kalibrovať strednú polohu servomotorov (pomocou klávesu '''H''')
+
!klaves!!servo/funkcia
** pomocou + a - nájdeme strednú polohu serva
+
|-
** enter potvrdí hodnotu a pokračuje sa ďalším servom  
+
|a/q || ľavá ruka
** 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'''
+
|;/p || pravá ruka
*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 hore na kláves '''R'''
+
|z/x || ľavá noha
*načíta choreografiu po klávese '''@''',  
+
|-
*zobrazí načítanú choreografiu na kláves '''?''' a
+
|,/. || pravá noha
*zatancuje načítanú choreografiu - kláves '''t'''.
+
|-
*Na klávesách '''3-6''' je možné nastaviť rôznu kombináciu (funkcie kombinacia1() - kombinacia4()).
+
|d/c || ľavá päta
*Kláves '''U''' testuje ultrazvuk
+
|-
*Kláves '''S''' zobrazí aktuálnu polohu servomotorov
+
|k/m || pravá päta
*'''Medzera''' resetne všetky servo motory do strednej polohy.
+
|-
 +
|1/9 || zníž/zvýš rýchlosť pohybu
 +
|-
 +
|H || kalibrovať strednú polohu servomotorov<br>
 +
* 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)
 +
|-
 +
|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
 +
|-
 +
|minus || zvukový pozdrav
 +
|-
 +
|R || ruky hore-dole
 +
|-
 +
|@ || načítanie choreografie, pozri [[Otto - príklady choreografií|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
 +
|-
 +
|} 
  
 
Pozrite si (a skopírujte si) príklady choreografií: [[Otto - príklady choreografií]]
 
Pozrite si (a skopírujte si) príklady choreografií: [[Otto - príklady choreografií]]
 +
 +
Download: [[Media:otto3.ino|otto3.ino]]
  
 
<syntaxhighlight lang="C" line=line>
 
<syntaxhighlight lang="C" line=line>
 
#include <Servo.h>
 
#include <Servo.h>
 
#include <EEPROM.h>
 
#include <EEPROM.h>
 +
#include <avr/pgmspace.h>
  
 
#define ECHO_BT_TO_USB 1
 
#define ECHO_BT_TO_USB 1
Line 126: Line 182:
  
 
static volatile int16_t distance;
 
static volatile int16_t distance;
 
uint8_t *melodia;
 
uint16_t dlzka_melodie;
 
  
 
#define FIS3 2960
 
#define FIS3 2960
Line 136: Line 189:
 
int16_t music_speed;
 
int16_t music_speed;
  
uint16_t dlzka_popcorn = 390;
+
//popcorn
uint8_t popcorn[] = { 149, 49,  
+
uint16_t dlzka_melodia1 = 390;
 +
const uint8_t melodia1[] PROGMEM = { 149, 49,  
 
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,
 
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,
 
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,
 
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,
Line 156: Line 210:
 
   init_serial(9600);
 
   init_serial(9600);
 
   init_ultrasonic();
 
   init_ultrasonic();
 
  melodia = popcorn;
 
  dlzka_melodie = dlzka_popcorn;
 
  
 
   randomSeed(analogRead(1));
 
   randomSeed(analogRead(1));
Line 194: Line 245:
 
     if (pohyb_znakom(z)) return;   
 
     if (pohyb_znakom(z)) return;   
 
     else if (pohyb_kombinacia(z)) return;  
 
     else if (pohyb_kombinacia(z)) return;  
 +
    else if (vydaj_zvukovy_efekt(z)) return;
 
     else if (z == '@') nacitaj_choreografiu();
 
     else if (z == '@') nacitaj_choreografiu();
 
     else if (z == '?') vypis_choreografiu();
 
     else if (z == '?') vypis_choreografiu();
Line 211: Line 263:
 
     else if (z == 'U') test_ultrazvuk();
 
     else if (z == 'U') test_ultrazvuk();
 
     else if (z == 'S') zobraz_stav();
 
     else if (z == 'S') zobraz_stav();
 +
   
 
   }
 
   }
 
   int16_t d = measure_distance();
 
   int16_t d = measure_distance();
 
   if (d < 10) menu_ultrasonic_request();
 
   if (d < 10) menu_ultrasonic_request();
 +
}
 +
 +
const uint8_t klavesy_efektov[] PROGMEM = {'v', 'b', 'n', 'j', 'h', 'g', 'f', 's', 'l', 'o', 'i', 'u', 'y', 'r', 'e', 'w'};
 +
#define POCET_KLAVES_EFEKTOV 16
 +
 +
uint8_t vydaj_zvukovy_efekt(char z)
 +
{
 +
  for (int i = 0; i < POCET_KLAVES_EFEKTOV; i++)
 +
    if (z == pgm_read_byte_near(klavesy_efektov + i)) zvukovy_efekt(i + 1);
 
}
 
}
  
Line 228: Line 290:
 
   {
 
   {
 
     *freq = FIS3;
 
     *freq = FIS3;
*del = music_speed;
+
    *del = music_speed;
 
   }
 
   }
 
   else if (*note == 255)
 
   else if (*note == 255)
 
   {
 
   {
 
     *freq = G3;
 
     *freq = G3;
*del = music_speed;
+
    *del = music_speed;
 
   }
 
   }
 
   else
 
   else
Line 252: Line 314:
 
         octave--;   
 
         octave--;   
 
       }
 
       }
  *freq = (uint16_t)ffreq;
+
    *freq = (uint16_t)ffreq;
 
     }
 
     }
else *freq = 0;
+
  else *freq = 0;
 
   }
 
   }
 
}
 
}
Line 267: Line 329:
 
}
 
}
  
void zahraj_melodiu()  
+
void zahraj_melodiu1()  
 
{
 
{
 +
  uint8_t nota[5];
 
   music_speed = 800 / 16;
 
   music_speed = 800 / 16;
 
   for (int j = 0; j < 4; j++)
 
   for (int j = 0; j < 4; j++)
 
   {
 
   {
     for (int i = 0; i < dlzka_melodie; i++)
+
     for (int i = 0; i < dlzka_melodia1; i++)
 
     {
 
     {
       otto_tone(melodia + i);
+
       nota[0] = pgm_read_byte_near(melodia1 + i);
       if (melodia[i] == 251) i += 4;
+
       if (nota[0] == 251)
 +
      {
 +
        for (int k = 1; k < 5; k++)
 +
          nota[k] = pgm_read_byte_near(melodia1 + i + k);       
 +
        i += 4;
 +
      }
 +
      otto_tone(nota);     
 
     }
 
     }
    music_speed -= 150 / 16;
 
 
   }
 
   }
   delay(3000);
+
}
 +
 
 +
void efekt1()
 +
{
 +
  uint16_t fre = 100;
 +
  for (int i = 0; i < 200; i++)
 +
  {
 +
    tone(SIRENA, fre);
 +
    fre += 10;
 +
    delay(2);
 +
  }
 +
  noTone(SIRENA);
 +
}
 +
 
 +
void efekt2()
 +
{
 +
  uint16_t fre = 2000;
 +
  for (int i = 0; i < 200; i++)
 +
  {
 +
    tone(SIRENA, fre);
 +
    fre -= 10;
 +
    delay(2);
 +
  }
 +
  noTone(SIRENA);
 +
}
 +
 
 +
void efekt3()
 +
{
 +
  uint16_t fre;
 +
  for (int i = 0; i < 200; i++)
 +
  {
 +
    fre = random(3000) + 20;
 +
    tone(SIRENA, fre);   
 +
    delay(2);
 +
  }
 +
  noTone(SIRENA);
 +
}
 +
 
 +
void efekt4()
 +
{
 +
  uint16_t fre = 100;
 +
  for (int i = 0; i < 200; i++)
 +
  {
 +
    (i&1) ? tone(SIRENA, fre) : tone(SIRENA, 2100 - fre);
 +
    fre += 10;
 +
    delay(2);
 +
  }
 +
  noTone(SIRENA);
 +
}
 +
 
 +
void efekt5()
 +
{
 +
  uint16_t fre = 100;
 +
  for (int i = 0; i < 100; i++)
 +
  {
 +
    (i&1) ? tone(SIRENA, fre) : tone(SIRENA, 2100 - fre);
 +
    fre += 20;
 +
    delay(4);
 +
  }
 +
  noTone(SIRENA);
 +
}
 +
 
 +
void efekt6()
 +
{
 +
  uint16_t d = 12;
 +
  for (int i = 0; i < 20; i++)
 +
  {
 +
    tone(SIRENA, 1760, 10);   
 +
    delay(d);
 +
    d += 3;
 +
  }
 +
}
 +
 
 +
void efekt7()
 +
{
 +
  for (int i = 0; i < 40; i++)
 +
  {
 +
    (i&1) ? noTone(SIRENA): tone(SIRENA, 1760);
 +
    delay(10);
 +
  }
 +
  noTone(SIRENA);
 +
}
 +
 
 +
void efekt8()
 +
{
 +
  uint16_t d = 72;
 +
  for (int i = 0; i < 20; i++)
 +
  {
 +
    tone(SIRENA, 1760, 10);   
 +
    delay(d);
 +
    d -= 3;
 +
  }
 +
}
 +
 
 +
void efekt9()
 +
{
 +
  float fre = 3500;
 +
  while (fre > 15)
 +
   {
 +
    tone(SIRENA, (uint16_t)fre);
 +
    fre *= 0.97;   
 +
    delay(2);
 +
  }
 +
  noTone(SIRENA);
 +
}
 +
 
 +
void efekt10()
 +
{
 +
  float fre = 55;
 +
  while (fre < 3000)
 +
  {
 +
    tone(SIRENA, (uint16_t)fre);
 +
    fre *= 1.03;   
 +
    delay(2);
 +
  }
 +
  noTone(SIRENA);
 +
}
 +
 
 +
void efekt11()
 +
{
 +
  uint16_t fre = 3500;
 +
  uint16_t d = 42;
 +
  for (int i = 0; i < 20; i++)
 +
  {
 +
    tone(SIRENA, fre);
 +
    fre -= 165;
 +
    delay(d);
 +
    d -= 2;
 +
  }
 +
  noTone(SIRENA);
 +
}
 +
 
 +
void efekt12()
 +
{
 +
  float fre = 110;
 +
  uint8_t d = 42;
 +
  for (int i = 0; i < 20; i++)
 +
  {
 +
    tone(SIRENA, fre);
 +
    fre += 165;
 +
    delay(d);
 +
    d -= 2;
 +
  }
 +
  noTone(SIRENA);
 +
}
 +
 
 +
void efekt13()
 +
{
 +
  uint16_t fre = 3400;
 +
  uint8_t d = 200;
 +
  uint8_t delta = 1;
 +
  for (int i = 0; i < 20; i++)
 +
  {
 +
    tone(SIRENA, fre);
 +
    fre -= 165;
 +
    delay(d);
 +
    d -= delta;
 +
    delta++;
 +
  }
 +
  tone(SIRENA, 110);
 +
  delay(1000);
 +
  noTone(SIRENA);
 +
}
 +
 
 +
void efekt14()
 +
{
 +
  uint16_t fre = 880; 
 +
  int16_t d = 20;
 +
  for (int i = 0; i < 20; i++)
 +
  {
 +
    tone(SIRENA, fre);
 +
    delay(d);
 +
    fre += random(50) - 25;
 +
    d += random(10) - 5;
 +
    if (d < 0) d = 1;
 +
  }
 +
  noTone(SIRENA);
 +
}
 +
 
 +
void efekt15()
 +
{
 +
  uint16_t fre = 440; 
 +
  int16_t d = 20;
 +
  for (int i = 0; i < 20; i++)
 +
  {
 +
    tone(SIRENA, fre);
 +
    delay(d);
 +
    fre += random(25) - 12;
 +
    d += random(10) - 5;
 +
    if (d < 0) d = 1;
 +
  }
 +
  noTone(SIRENA);
 +
}
 +
 
 +
void efekt16()
 +
{
 +
  uint16_t fre = 1760; 
 +
  int16_t d = 20;
 +
  for (int i = 0; i < 20; i++)
 +
  {
 +
    tone(SIRENA, fre);
 +
    delay(d);
 +
    fre += random(100) - 50;
 +
    d += random(10) - 5;
 +
    if (d < 0) d = 1;
 +
  }
 +
  noTone(SIRENA);
 +
}
 +
 
 +
void zvukovy_efekt(uint8_t cislo)
 +
{
 +
  switch (cislo) {
 +
    case 1: efekt1(); break;
 +
    case 2: efekt2(); break;
 +
    case 3: efekt3(); break;
 +
    case 4: efekt4(); break;
 +
    case 5: efekt5(); break;
 +
    case 6: efekt6(); break;
 +
    case 7: efekt7(); break;
 +
    case 8: efekt8(); break;
 +
    case 9: efekt9(); break;
 +
    case 10: efekt10(); break;
 +
    case 11: efekt11(); break;
 +
    case 12: efekt12(); break;
 +
    case 13: efekt13(); break; 
 +
    case 14: efekt14(); break; 
 +
    case 15: efekt15(); break; 
 +
    case 16: efekt16(); break; 
 +
  }
 
}
 
}
  
Line 307: Line 603:
 
     if (d >= 15) count2++;
 
     if (d >= 15) count2++;
 
     else count2 = 0;
 
     else count2 = 0;
if (serial_available() || Serial.available()) return;
+
    if (serial_available() || Serial.available()) return;
 
   }
 
   }
 
   if (millis() - tm >= 1500)
 
   if (millis() - tm >= 1500)
Line 351: Line 647:
 
{
 
{
 
   if (cmd == 1) vpred();
 
   if (cmd == 1) vpred();
   if (cmd == 2) zahraj_melodiu();
+
   if (cmd == 2) zahraj_melodiu1();
 
   if (cmd == 3) melodia_jedna_druhej();
 
   if (cmd == 3) melodia_jedna_druhej();
 
   if (cmd == 4) ahoj();
 
   if (cmd == 4) ahoj();
Line 624: Line 920:
 
     serial_println_num(ch_val[i]);       
 
     serial_println_num(ch_val[i]);       
 
   }
 
   }
 +
  serial_println("---");
 
   pipni();
 
   pipni();
 
}
 
}
Line 632: Line 929:
 
   {
 
   {
 
     delay(ch_time[i]);
 
     delay(ch_time[i]);
     nastav_koncatinu(ch_servo[i], ch_val[i]);
+
     if (ch_servo[i] < 10) nastav_koncatinu(ch_servo[i], ch_val[i]);
 +
    else specialny_prikaz(ch_servo[i], ch_val[i]);
 
   }
 
   }
 
   pipni();
 
   pipni();
 
   while (serial_available()) serial_read();
 
   while (serial_available()) serial_read();
 
   while (Serial.available()) Serial.read();
 
   while (Serial.available()) Serial.read();
 +
}
 +
 +
void specialny_prikaz(uint8_t prikaz, uint8_t argument)
 +
{
 +
  if (prikaz == 10) spomalenie = argument;
 +
  else if (prikaz == 11) zacni_hrat_melodiu(argument);
 +
  else if (prikaz == 12) zvukovy_efekt(argument);
 +
}
 +
 +
void zacni_hrat_melodiu(uint8_t cislo)
 +
{
 +
 
 
}
 
}
  

Revision as of 22:11, 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.

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
  • 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)
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
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

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