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

From DT^2
Jump to: navigation, search
Line 110: Line 110:
 
|-
 
|-
 
|V || vypni/zapni režim nudy
 
|V || vypni/zapni režim nudy
 +
|-
 +
|A || automatický štart choreografie po zapnutí
 
|-
 
|-
 
|}
 
|}
Line 161: Line 163:
  
 
// ak su niektore serva naopak, je tu jednotka
 
// ak su niektore serva naopak, je tu jednotka
−
uint8_t servo_invertovane[6] = {0, 0, 1, 1, 0, 1};
+
uint8_t servo_invertovane[6] = {0, 0, 0, 0, 0, 0};
  
 
// znaky, ktorymi sa ovladaju jednotlive stupne volnosti
 
// znaky, ktorymi sa ovladaju jednotlive stupne volnosti
 
char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' };
 
char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' };
 
// co robia jednotlive znaky (znamienko urcuje smer)
 
// co robia jednotlive znaky (znamienko urcuje smer)
−
int8_t zmeny[] = {SERVO_LAVA_RUKA, -SERVO_LAVA_RUKA,
+
int8_t zmeny[] = {-SERVO_LAVA_RUKA, SERVO_LAVA_RUKA,
 
                   SERVO_PRAVA_RUKA, -SERVO_PRAVA_RUKA,
 
                   SERVO_PRAVA_RUKA, -SERVO_PRAVA_RUKA,
 
                   -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA,
 
                   -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA,
 
                   -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA,
 
                   -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA,
 
                   SERVO_LAVA_PATA, -SERVO_LAVA_PATA,
 
                   SERVO_LAVA_PATA, -SERVO_LAVA_PATA,
−
                   SERVO_PRAVA_PATA, -SERVO_PRAVA_PATA
+
                   -SERVO_PRAVA_PATA, SERVO_PRAVA_PATA
 
                 };
 
                 };
  
Line 190: Line 192:
 
int krok;
 
int krok;
 
uint8_t spomalenie;
 
uint8_t spomalenie;
 +
uint8_t auto_start;
  
 
static volatile int16_t distance;
 
static volatile int16_t distance;
Line 198: Line 201:
 
   Serial.begin(9600);
 
   Serial.begin(9600);
 
   init_tone2();
 
   init_tone2();
−
   init_serial(9600);
+
   init_serial(57600);
 
   init_ultrasonic();
 
   init_ultrasonic();
  
Line 220: Line 223:
 
   spomalenie = 6;
 
   spomalenie = 6;
 
   ahoj();
 
   ahoj();
−
   ruky2();
+
   ruky();
 
   delay(100);
 
   delay(100);
 
   serial_println_flash(PSTR("\r\n  Otto DTDT"));
 
   serial_println_flash(PSTR("\r\n  Otto DTDT"));
 +
  if (auto_start > 0)
 +
  {
 +
    delay(7000);
 +
    while (serial_available()) serial_read();
 +
    while (Serial.available()) Serial.read();
 +
    precitaj_choreografiu_z_EEPROM(auto_start);
 +
    zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
 +
  }
 
   cas_ked_naposledy_stlacil = millis();
 
   cas_ked_naposledy_stlacil = millis();
 
}
 
}
Line 260: Line 271:
 
     else if (z == 'X') skus_naformatovat_EEPROM_choreografie();
 
     else if (z == 'X') skus_naformatovat_EEPROM_choreografie();
 
     else if (z == 'C') skus_nacitat_choreografiu_z_EEPROM();
 
     else if (z == 'C') skus_nacitat_choreografiu_z_EEPROM();
−
     else if (z == 'V') {
+
     else if (z == 'A') nastav_automaticky_start();
−
      vypni_nudu ^= 1;
+
    else if (z == 'V') prepni_nudu();
−
      serial_print("ticho:");
 
−
      serial_println_num(vypni_nudu);
 
−
    }
 
 
   }
 
   }
 
   int16_t d = measure_distance();
 
   int16_t d = measure_distance();
Line 270: Line 278:
  
 
   skus_ci_sa_nenudi();
 
   skus_ci_sa_nenudi();
 +
}
 +
 +
void nastav_automaticky_start()
 +
{
 +
  serial_print_flash(PSTR("Automaticky start [1,2,3, 0=vypni]:"));
 +
  char z;
 +
  do { z = read_char(); } while ((z < '0') || (z > '3'));
 +
  if (z == '0') EEPROM.write(1, '~');
 +
  else EEPROM.write(1, z);
 +
  serial_println_char(z);
 +
}
 +
 +
void prepni_nudu()
 +
{
 +
  vypni_nudu ^= 1;
 +
  serial_print_flash(PSTR("ticho:"));
 +
  serial_println_num(vypni_nudu);
 
}
 
}
  
Line 708: Line 733:
 
// 0: slot number where choreography 3 starts (B3. Real address = B3 x 4)
 
// 0: slot number where choreography 3 starts (B3. Real address = B3 x 4)
 
// 1: marker '~' indicates calibration is already stored in EEPROM and will be loaded on startup
 
// 1: marker '~' indicates calibration is already stored in EEPROM and will be loaded on startup
 +
//          '1', '2', '3' indicate the same, but choreography 1,2, or 3 is automatically launched on startup
 
// 2-8: servo calibration (central points to be used instead of default 90)
 
// 2-8: servo calibration (central points to be used instead of default 90)
 
// 9-14: lower limit for direct control for all 6 servos
 
// 9-14: lower limit for direct control for all 6 servos
Line 833: Line 859:
 
{
 
{
 
   uint8_t value = EEPROM.read(1);
 
   uint8_t value = EEPROM.read(1);
−
   if (value != '~') return;
+
   if ((value != '~') && (value != '1') && (value != '2') && (value != '3')) return;
 +
  if (value != '~') auto_start = value - '0';
 +
  else auto_start = 0;
 
   for (int i = 2; i < 8; i++)
 
   for (int i = 2; i < 8; i++)
−
     prednastavena_kalibracia[i - 2] = EEPROM.read(i);
+
  {
 +
    int16_t k = EEPROM.read(i);
 +
    if (k > 127) servo_invertovane[i - 2] = 1;
 +
    else servo_invertovane[i - 2] = 0;
 +
    k &= 127;
 +
     prednastavena_kalibracia[i - 2] = k + (90-63);
 +
  }
 
   for (int i = 0; i < 6; i++)
 
   for (int i = 0; i < 6; i++)
 
     dolny_limit[i] = EEPROM.read(i + 9);
 
     dolny_limit[i] = EEPROM.read(i + 9);
Line 856: Line 890:
 
   if (odpoved == 'Y')
 
   if (odpoved == 'Y')
 
   {
 
   {
−
     EEPROM.write(1, '~');
+
     char kalib_state = EEPROM.read(1);
 +
    if ((kalib_state == '~') || ((kalib_state >= '1') && (kalib_state <= '3')))
 +
      EEPROM.write(1, kalib_state);
 +
    else EEPROM.write(1, '~');
 
     for (int i = 2; i < 8; i++)
 
     for (int i = 2; i < 8; i++)
−
       EEPROM.write(i, kalib[i - 2]);
+
    {
 +
      int16_t k = kalib[i - 2] - (90-63);
 +
      if (k < 0) k = 0;
 +
      else if (k > 127) k = 127;
 +
      if (servo_invertovane[i - 2]) k += 128;     
 +
       EEPROM.write(i, (uint8_t)k);
 +
    }
 
     for (int i = 0; i < 6; i++)
 
     for (int i = 0; i < 6; i++)
 
       EEPROM.write(9 + i, dolny_limit[i]);
 
       EEPROM.write(9 + i, dolny_limit[i]);
Line 876: Line 919:
 
void ruky()
 
void ruky()
 
{
 
{
−
   int odloz_krok = krok;
+
   uint8_t odloz_spomalenie = spomalenie;
−
   delay(500);
+
   spomalenie = 0;
−
  krok = 90;
+
   nastav_koncatinu(5, 0);
−
  pohyb(SERVO_LAVA_RUKA);
+
   nastav_koncatinu(6, 0);
−
  pohyb(SERVO_PRAVA_RUKA);
 
−
  delay(1000);
 
−
  krok = 180;
 
−
  pohyb(-SERVO_LAVA_RUKA);
 
−
  pohyb(-SERVO_PRAVA_RUKA);
 
−
  delay(1000);
 
−
  krok = odloz_krok;
 
−
  pipni();
 
−
}
 
−
 
 
−
void ruky2()
 
−
{
 
−
  int odloz_krok = krok;
 
−
  delay(500);
 
−
  krok = 180;
 
−
   pohyb(SERVO_LAVA_RUKA);
 
−
   pohyb(SERVO_PRAVA_RUKA);
 
 
   delay(1000);
 
   delay(1000);
−
   krok = 90;
+
   nastav_koncatinu(5, 90);
−
   pohyb(-SERVO_LAVA_RUKA);
+
   nastav_koncatinu(6, 90);
−
   pohyb(-SERVO_PRAVA_RUKA);
+
   spomalenie = odloz_spomalenie;
 
   delay(1000);
 
   delay(1000);
−
  krok = odloz_krok;
 
 
   pipni();
 
   pipni();
 
}
 
}
Line 924: Line 949:
 
}
 
}
  
−
void nastav_koncatinu(int8_t servo, uint8_t poloha)
+
void nastav_koncatinu(int8_t srv, uint8_t poloha)
 
{
 
{
 
   int8_t delta;
 
   int8_t delta;
−
  int8_t srv = (servo > 0) ? servo : -servo;
 
 
   srv--;
 
   srv--;
−
     
+
 
 +
  if (servo_invertovane[srv]) poloha = 180 - poloha;
 +
 
 
   if ((int16_t)poloha + (int16_t)kalib[srv] - 90 < 0) poloha = 0;
 
   if ((int16_t)poloha + (int16_t)kalib[srv] - 90 < 0) poloha = 0;
 
   else poloha += kalib[srv] - 90;
 
   else poloha += kalib[srv] - 90;
Line 935: Line 961:
 
   if (poloha > 180) poloha = 180;
 
   if (poloha > 180) poloha = 180;
  
−
   if (stav[srv] < poloha) delta = 1;
+
   if (spomalenie == 0) {
−
  else delta = -1;
+
     stav[srv] = poloha;
−
  while (stav[srv] != poloha)
 
−
  {
 
−
     stav[srv] += delta;
 
 
     s[srv].write(stav[srv]);
 
     s[srv].write(stav[srv]);
−
     delay(spomalenie);
+
  }
 +
  else { 
 +
     if (stav[srv] < poloha) delta = 1;
 +
    else delta = -1;
 +
    while (stav[srv] != poloha)
 +
    {
 +
      stav[srv] += delta;
 +
      s[srv].write(stav[srv]);
 +
      delay(spomalenie);
 +
    }
 
   }
 
   }
 
}
 
}
Line 1,068: Line 1,100:
 
   pipni();
 
   pipni();
 
}
 
}
 +
 +
uint32_t koniec_tanca;
  
 
void zatancuj_choreografiu(uint16_t *ch_time, uint8_t *ch_servo, uint8_t *ch_val, int ch_len )
 
void zatancuj_choreografiu(uint16_t *ch_time, uint8_t *ch_servo, uint8_t *ch_val, int ch_len )
 
{
 
{
 +
  koniec_tanca = millis() + 3600000;
 
   for (int i = 0; i < ch_len - 1; i++)
 
   for (int i = 0; i < ch_len - 1; i++)
 
   {
 
   {
 
     delay(ch_time[i]);
 
     delay(ch_time[i]);
−
     if (ch_servo[i] < 9) nastav_koncatinu(ch_servo[i], ch_val[i]);
+
    if (millis() > koniec_tanca) break;
 +
     if (ch_servo[i] < 7) nastav_koncatinu(ch_servo[i], ch_val[i]);
 
     else i = specialny_prikaz(i, ch_servo[i], ch_val[i]);
 
     else i = specialny_prikaz(i, ch_servo[i], ch_val[i]);
 
     if (serial_available() || Serial.available()) break;
 
     if (serial_available() || Serial.available()) break;
Line 1,083: Line 1,119:
 
int specialny_prikaz(uint16_t i, uint8_t prikaz, uint8_t argument)
 
int specialny_prikaz(uint16_t i, uint8_t prikaz, uint8_t argument)
 
{
 
{
−
   if (prikaz == 9) return ((int)argument) - 1;
+
   if (prikaz == 8) koniec_tanca = millis() + 1000 * (uint32_t)argument;
 +
  else if (prikaz == 9) return ((int)argument) - 1;
 
   else if (prikaz == 10) spomalenie = argument;
 
   else if (prikaz == 10) spomalenie = argument;
 
   else if (prikaz == 11) zahraj_melodiu(argument);
 
   else if (prikaz == 11) zahraj_melodiu(argument);
Line 1,101: Line 1,138:
 
}
 
}
  
−
uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
+
uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota, uint8_t min_hodnota, uint8_t max_hodnota)
 
{
 
{
 
   serial_print_flash(PSTR(" (+/-/ENTER): "));
 
   serial_print_flash(PSTR(" (+/-/ENTER): "));
Line 1,109: Line 1,146:
 
   do {
 
   do {
 
     z = read_char();
 
     z = read_char();
−
     if ((z == '+') && (hodnota < 180)) hodnota++;
+
     if ((z == '+') && (hodnota < max_hodnota)) hodnota++;
−
     else if ((z == '-') && (hodnota > 0)) hodnota--;
+
     else if ((z == '-') && (hodnota > min_hodnota)) hodnota--;
 
     if ((z == '+') || (z == '-'))
 
     if ((z == '+') || (z == '-'))
 
     {
 
     {
Line 1,125: Line 1,162:
 
   {
 
   {
 
     serial_print_num(i);
 
     serial_print_num(i);
−
     kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
+
     kalib[i] = nalad_hodnotu_serva(i, kalib[i], 90-63, 90+63);
 
     serial_print_num(i);
 
     serial_print_num(i);
 
     serial_print(": ");
 
     serial_print(": ");
Line 1,134: Line 1,171:
 
     serial_print(" ");
 
     serial_print(" ");
 
   }
 
   }
 +
  serial_print_flash(PSTR(" Chyt robota zozadu za telo (ENTER):"));
 +
  char z;
 +
  do { z = read_char(); } while (z != 13);
 +
  serial_println(z);
 +
 +
  serial_print_flash(PSTR("*** Nasledujuce koncatiny su lave/prave z pohladu robota.\r\nAk to nesedi, treba prehodit kabliky servo!\r\n"));
 +
 +
  for (int i = 0; i < 6; i++) servo_invertovane[i] = 0;
 +
 +
  nastav_koncatinu(SERVO_PRAVA_PATA, 0);
 +
  serial_print_flash(PSTR("Prava pata: je von alebo dnu? (V/D):"));
 +
  do { z = read_char(); } while ((z != 'V') && (z != 'D'));
 +
  serial_println_char(z);
 +
  nastav_koncatinu(SERVO_PRAVA_PATA, 90);
 +
  if (z == 'D') servo_invertovane[SERVO_PRAVA_PATA - 1] = 0;
 +
  else servo_invertovane[SERVO_PRAVA_PATA - 1] = 1;
 +
 
 +
  nastav_koncatinu(SERVO_LAVA_PATA, 0);
 +
  serial_print_flash(PSTR("Lava pata: je von alebo dnu? (V/D):"));
 +
  do { z = read_char(); } while ((z != 'V') && (z != 'D'));
 +
  serial_println_char(z);
 +
  nastav_koncatinu(SERVO_LAVA_PATA, 90);
 +
  if (z == 'V') servo_invertovane[SERVO_LAVA_PATA - 1] = 0;
 +
  else servo_invertovane[SERVO_LAVA_PATA - 1] = 1;
 +
 
 +
  nastav_koncatinu(SERVO_PRAVA_NOHA, 0);
 +
  serial_print_flash(PSTR("Prava noha: vpredu je pata alebo spicka? (P/S):"));
 +
  do { z = read_char(); } while ((z != 'P') && (z != 'S'));
 +
  serial_println_char(z);
 +
  nastav_koncatinu(SERVO_PRAVA_NOHA, 90);
 +
  if (z == 'P') servo_invertovane[SERVO_PRAVA_NOHA - 1] = 1;
 +
  else servo_invertovane[SERVO_PRAVA_NOHA - 1] = 0;
 +
 +
  nastav_koncatinu(SERVO_LAVA_NOHA, 0);
 +
  serial_print_flash(PSTR("Lava noha: vpredu je pata alebo spicka? (P/S):"));
 +
  do { z = read_char(); } while ((z != 'P') && (z != 'S'));
 +
  serial_println_char(z);
 +
  nastav_koncatinu(SERVO_LAVA_NOHA, 90);
 +
  if (z == 'S') servo_invertovane[SERVO_LAVA_NOHA - 1] = 1;
 +
  else servo_invertovane[SERVO_LAVA_NOHA - 1] = 0;
 +
 
 +
  nastav_koncatinu(SERVO_PRAVA_RUKA, 0);
 +
  serial_print_flash(PSTR("Prava ruka: je hore alebo dole? (H/D):"));
 +
  do { z = read_char(); } while ((z != 'H') && (z != 'D'));
 +
  serial_println_char(z);
 +
  nastav_koncatinu(SERVO_PRAVA_RUKA, 90);
 +
  if (z == 'H') servo_invertovane[SERVO_PRAVA_RUKA - 1] = 1;
 +
  else servo_invertovane[SERVO_PRAVA_RUKA - 1] = 0;
 +
 +
  nastav_koncatinu(SERVO_LAVA_RUKA, 0);
 +
  serial_print_flash(PSTR("Lava ruka: je hore alebo dole? (H/D):"));
 +
  do { z = read_char(); } while ((z != 'H') && (z != 'D'));
 +
  serial_println_char(z);
 +
  nastav_koncatinu(SERVO_LAVA_RUKA, 90);
 +
  if (z == 'D') servo_invertovane[SERVO_PRAVA_RUKA - 1] = 0;
 +
  else servo_invertovane[SERVO_PRAVA_RUKA - 1] = 1;
 +
 
   serial_println_flash(PSTR("ok"));
 
   serial_println_flash(PSTR("ok"));
 
   pipni();
 
   pipni();
 
}
 
}
−
 
+
 
void nastav_limity()
 
void nastav_limity()
 
{
 
{
Line 1,144: Line 1,238:
 
     serial_print_num(i);
 
     serial_print_num(i);
 
     serial_print_flash(PSTR("dolny"));
 
     serial_print_flash(PSTR("dolny"));
−
     dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
+
     dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i], 0, 180);
 
     serial_print_num(i);
 
     serial_print_num(i);
 
     serial_print_flash(PSTR(" dolny: "));
 
     serial_print_flash(PSTR(" dolny: "));
Line 1,152: Line 1,246:
 
     serial_print_num(i);
 
     serial_print_num(i);
 
     serial_print_flash(PSTR("horny"));
 
     serial_print_flash(PSTR("horny"));
−
     horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
+
     horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i], 0, 180);
 
     serial_print_num(i);
 
     serial_print_num(i);
 
     serial_print_flash(PSTR(" horny: "));
 
     serial_print_flash(PSTR(" horny: "));
Line 1,176: Line 1,270:
 
   }
 
   }
 
   serial_println();
 
   serial_println();
 +
 
 +
  serial_print_flash(PSTR("invertovane servo: "));
 +
  for (int i = 0; i < 6; i++)
 +
  {
 +
    serial_print_num(servo_invertovane[i]);
 +
    serial_print_char(' ');
 +
  }
 +
  serial_println();
 +
   
 
   serial_print_flash(PSTR("dolny limit: "));
 
   serial_print_flash(PSTR("dolny limit: "));
 
   for (int i = 0; i < 6; i++) {
 
   for (int i = 0; i < 6; i++) {

Revision as of 23:55, 11 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:

signál 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

1 HC-SR04 Front Back.jpeg

Čí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, treba potvrdiť veľkým Y
minus zvukový pozdrav
R ruky hore-dole
@ načítanie choreografie, pozri príklady a formát
? zobrazí načítanú choreografiu (kontrola po @)
t zatancuje načítanú choreografiu
U testuje ultrazvuk
S zobrazí aktuálnu polohu servomotorov (vhodné pri laden
medzera reset všetkých motorov do strednej polohy
3,...,6 zmiešaný pohyb (treba nastaviť vo funkciách kombinacia1() - kombinacia4())
v,b,n,j,h,g,f,s,l,o,i,u,y,r,e,w zvukové efekty 1-16
TAB,*,/,+ zahrá melódiu 1-4
W zapísať choreografiu do EEPROM (opatrne)
C načítať choreografiu z EEPROM
X formátovať EEPROM na zápis choreografií (opatrne)
V vypni/zapni režim nudy
A automatický štart choreografie po zapnutí

Pozrite si (a skopírujte si) príklady choreografií: Otto - príklady choreografií

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