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

From DT^2
Jump to: navigation, search
 
(8 intermediate revisions by 2 users not shown)
Line 1: Line 1:
Program umožňuje priame riadenie robota cez BlueTooth:
+
Program umožňuje priame riadenie robota (aj cez BlueTooth).
 +
Zatiaľ musíme pripájať BT na piny 0 a 1 (namiesto 2 a 4) a počas programovania vytiahnuť káblik označený TXD na strane BT modulu.
 +
 
 +
Na komunikáciu s robotom môžete použiť napr. program Putty: [https://the.earth.li/~sgtatham/putty/latest/w64/putty.exe putty.exe].
  
 
{|class="wikitable"
 
{|class="wikitable"
Line 15: Line 18:
 
|-
 
|-
 
|k/m || pravá päta || 3
 
|k/m || pravá päta || 3
 +
|-
 +
|1/9 || zníž/zvýš rýchlosť pohybu || -
 
|-
 
|-
 
|Tx  || BT|| 4
 
|Tx  || BT|| 4
Line 28: Line 33:
  
 
Ďalej umožňuje  
 
Ďalej umožňuje  
*kalibrovať strednú polohu servomotorov (pomocou klávesu '''H'''),
+
*kalibrovať strednú polohu servomotorov (pomocou klávesu '''H''')
*pozdraví po klávese '''-''',
+
** pomocou + a - najdeme nulovu polohu serva
 +
** eneter ideme na dalsie servo
 +
** ukoncime E ulozenie do eprom
 +
** potvrdime Y
 +
*kalibrovať limity pre všetky stupne voľnosti (pomocou klávesu '''J'''), zobrazenie celej kalibrácie: kláves '''G'''
 +
*celú kalibráciu je možné zapísať do trvalej pamäte EEPROM pomocou klávesu '''E''' (používať opatrne!), po zapnutí sa automaticky načíta
 +
*pozdraví po klávese '''-''', ruky dá hore na kláves '''R'''
 
*načíta choreografiu po klávese '''@''',  
 
*načíta choreografiu po klávese '''@''',  
 
*zobrazí načítanú choreografiu na kláves '''?''' a  
 
*zobrazí načítanú choreografiu na kláves '''?''' a  
Line 38: Line 49:
 
''Pre komunikáciu na pinoch 0,1 pomocou bežného sériového portu treba pridať riadok
 
''Pre komunikáciu na pinoch 0,1 pomocou bežného sériového portu treba pridať riadok
 
<code>#define pc Serial</code>
 
<code>#define pc Serial</code>
a zakomentovať riadok pc.begin(...);''
+
a zakomentovať riadky <code>pc.begin(...);</code> a <code>SoftwareSerial pc(...)</code>''
  
 
<syntaxhighlight lang="C" line=line>
 
<syntaxhighlight lang="C" line=line>
#include <SoftwareSerial.h>
+
//#include <SoftwareSerial.h>
 
#include <Servo.h>
 
#include <Servo.h>
 +
#include <EEPROM.h>
 +
 +
#define pc Serial
  
 
#define US_TRIG  7
 
#define US_TRIG  7
Line 50: Line 64:
 
#define BT_TX  4
 
#define BT_TX  4
  
//ktore servo je na ktorom pine arduina
+
#define SIRENA 12
 +
 
 +
//maximalna dlzka choreografie
 +
#define CHOREO_LEN 200
 +
 
 +
// cisla pinov, kde su zapojene servo motory
 +
#define PIN_SERVO_LAVA_RUKA  10
 +
#define PIN_SERVO_PRAVA_RUKA  11
 +
#define PIN_SERVO_LAVA_NOHA  9
 +
#define PIN_SERVO_PRAVA_NOHA  6
 +
#define PIN_SERVO_LAVA_PATA  5
 +
#define PIN_SERVO_PRAVA_PATA  3
 +
 
 
#define S1 3
 
#define S1 3
 
#define S2 5
 
#define S2 5
Line 75: Line 101:
 
// 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' };
int8_t zmeny[] = {-SERVO_LAVA_RUKA, SERVO_LAVA_RUKA,  
+
// co robia jednotlive znaky (znamienko urcuje smer)
                   -SERVO_PRAVA_RUKA, SERVO_PRAVA_RUKA,  
+
int8_t zmeny[] = {SERVO_LAVA_RUKA, -SERVO_LAVA_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,  
Line 83: Line 110:
  
 
// sem si mozno ulozit svoju kalibraciu
 
// sem si mozno ulozit svoju kalibraciu
uint8_t prednastavena_kalibracia[] = { 78, 69, 83, 80, 50, 67 };
+
//uint8_t prednastavena_kalibracia[] = { 78, 69, 83, 80, 50, 67 };
//uint8_t prednastavena_kalibracia = { 90, 90, 90, 90, 90, 90 };
+
uint8_t prednastavena_kalibracia[] = { 90, 90, 90, 90, 90, 90 };
 +
 
 +
uint8_t dolny_limit[] = {0, 0, 0, 0, 0, 0, 0, 0};
 +
uint8_t horny_limit[] = {180, 180, 180, 180, 180, 180};
  
 
Servo s[6];
 
Servo s[6];
SoftwareSerial pc(BT_TX, BT_RX);
+
//SoftwareSerial pc(BT_TX, BT_RX);
  
 
uint16_t ch_time[CHOREO_LEN];
 
uint16_t ch_time[CHOREO_LEN];
Line 99: Line 129:
 
void setup() {
 
void setup() {
 
   randomSeed(analogRead(1));
 
   randomSeed(analogRead(1));
   s[0].attach(S1);
+
   s[0].attach(PIN_SERVO_PRAVA_PATA);
   s[1].attach(S2);
+
   s[1].attach(PIN_SERVO_LAVA_PATA);
   s[2].attach(S3);
+
   s[2].attach(PIN_SERVO_PRAVA_NOHA);
   s[3].attach(S4);
+
   s[3].attach(PIN_SERVO_LAVA_NOHA);
   s[4].attach(S5);
+
   s[4].attach(PIN_SERVO_PRAVA_RUKA);
   s[5].attach(S6);
+
   s[5].attach(PIN_SERVO_LAVA_RUKA);
 +
  precitaj_kalibraciu_z_EEPROM();
 
   for (int i = 0; i < 6; i++)
 
   for (int i = 0; i < 6; i++)
 
   {
 
   {
Line 111: Line 142:
 
     s[i].write(stav[i]);
 
     s[i].write(stav[i]);
 
   }
 
   }
   pc.begin(9600);
+
   // pc.begin(9600);
 
   Serial.begin(9600);
 
   Serial.begin(9600);
 
   ch_len = 0;
 
   ch_len = 0;
   krok = 1;
+
   krok = 7;
 
   ahoj();
 
   ahoj();
 
   ruky();
 
   ruky();
 +
}
 +
 +
void precitaj_kalibraciu_z_EEPROM()
 +
{
 +
  uint8_t value = EEPROM.read(1);
 +
  if (value != '~') return;
 +
  for (int i = 2; i < 8; i++)
 +
    prednastavena_kalibracia[i - 2] = EEPROM.read(i);
 +
  for (int i = 0; i < 6; i++)
 +
    dolny_limit[i] = EEPROM.read(i + 9);
 +
  for (int i = 0; i < 6; i++)
 +
    horny_limit[i] = EEPROM.read(i + 15);   
 +
}
 +
 +
void zapis_kalibraciu_do_EEPROM()
 +
{
 +
  pc.print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
 +
  while (!pc.available());
 +
  char odpoved = pc.read();
 +
  pc.println(odpoved);
 +
  if (odpoved == 'Y')
 +
  {
 +
    EEPROM.write(1, '~');
 +
    for (int i = 2; i < 8; i++)
 +
      EEPROM.write(i, kalib[i - 2]);
 +
    for (int i = 0; i < 6; i++)
 +
      EEPROM.write(9 + i, dolny_limit[i]);
 +
    for (int i = 0; i < 6; i++)
 +
      EEPROM.write(15 + i, horny_limit[i]);
 +
    pc.println("ok");
 +
  }
 
}
 
}
  
Line 158: Line 220:
 
   if (random(10) > 4) tone(SIRENA, 1357, 50);
 
   if (random(10) > 4) tone(SIRENA, 1357, 50);
 
   else tone(SIRENA, 1047, 50);
 
   else tone(SIRENA, 1047, 50);
 +
}
 +
 +
void nastav_koncatinu(int8_t servo, uint8_t poloha)
 +
{
 +
  int8_t srv = (servo > 0)?servo:-servo;
 +
  srv--;
 +
  poloha += kalib[srv] - 90;
 +
  if (poloha > 180) poloha = 180;
 +
  if (poloha < 0) poloha = 0;
 +
  stav[srv] = poloha;
 +
  s[srv].write(stav[srv]);
 
}
 
}
  
Line 167: Line 240:
 
   if (servo > 0)
 
   if (servo > 0)
 
   {
 
   {
     if (stav[srv] <= 180 - krok) stav[srv] += krok;
+
     if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
     else stav[srv] = 180;
+
     else stav[srv] = horny_limit[srv];
 
     s[srv].write(stav[srv]);
 
     s[srv].write(stav[srv]);
 
   }
 
   }
 
   else if (servo < 0)
 
   else if (servo < 0)
 
   {
 
   {
     if (stav[srv] >= krok) stav[srv] -= krok;  
+
     if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok;  
     else stav[srv] = 0;
+
     else stav[srv] = dolny_limit[srv];
 
     s[srv].write(stav[srv]);       
 
     s[srv].write(stav[srv]);       
 
   }
 
   }
Line 252: Line 325:
 
     ch_val[ch_len] = nacitajCislo();
 
     ch_val[ch_len] = nacitajCislo();
 
     ch_len++;
 
     ch_len++;
 +
  if (ch_len == CHOREO_LEN) break;
 
   } while (tm > 0);
 
   } while (tm > 0);
 
   pipni();   
 
   pipni();   
Line 274: Line 348:
 
   {
 
   {
 
     delay(ch_time[i]);
 
     delay(ch_time[i]);
     s[ch_servo[i]].write(ch_val[i]);
+
     nastav_koncatinu(ch_servo[i], ch_val[i]);
 
   }
 
   }
 +
  if (ch_len > 0) stav[ch_servo[ch_len - 1]] = ch_val[ch_len - 1];
 
   pipni();
 
   pipni();
 
}
 
}
Line 289: Line 364:
 
}
 
}
  
void kalibruj()
+
uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
 
{
 
{
  for (int i = 0; i < 6; i++)
 
  {
 
    pc.print(i);
 
 
     pc.print(" (+/-/ENTER): ");
 
     pc.print(" (+/-/ENTER): ");
     pc.println(kalib[i]);
+
     pc.println(hodnota);
     s[i].write(kalib[i]);
+
     s[servo].write(hodnota);
 
     char z;
 
     char z;
 
     do {
 
     do {
 
       z = pc.read();
 
       z = pc.read();
       if ((z == '+') && (kalib[i] < 180)) { kalib[i]++; pc.print(kalib[i]); pc.print('\r'); s[i].write(kalib[i]); }
+
       if ((z == '+') && (hodnota < 180)) hodnota++;
       else if ((z == '-') && (kalib[i] > 0)) { kalib[i]--; pc.print(kalib[i]); pc.print('\r'); s[i].write(kalib[i]); }
+
      else if ((z == '-') && (hodnota > 0)) hodnota--;  
 +
       if ((z == '+') || (z == '-'))
 +
      {
 +
        pc.print(hodnota); pc.print('\r');  
 +
        s[servo].write(hodnota);
 +
      }
 
     } while (z != 13);
 
     } while (z != 13);
 +
    return hodnota;
 +
}
 +
 +
void kalibruj()
 +
{
 +
  for (int i = 0; i < 6; i++)
 +
  {
 +
    pc.print(i);
 +
    kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
 
     pc.print(i);
 
     pc.print(i);
 
     pc.print(": ");
 
     pc.print(": ");
Line 310: Line 396:
 
   pc.println("ok");
 
   pc.println("ok");
 
   pipni();
 
   pipni();
 +
}
 +
 +
void nastav_limity()
 +
{
 +
  for (int i = 0; i < 6; i++)
 +
  {
 +
    pc.print(i);
 +
    pc.print("dolny");
 +
    dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
 +
    pc.print(i);
 +
    pc.print(" dolny: ");
 +
    pc.println(dolny_limit[i]);
 +
    s[i].write(kalib[i]);
 +
 +
    pc.print(i);
 +
    pc.print("horny");
 +
    horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
 +
    pc.print(i);
 +
    pc.print(" horny: ");
 +
    pc.println(horny_limit[i]);
 +
    s[i].write(kalib[i]);
 +
  }
 +
  for (int i = 0; i < 6; i++) { pc.print(dolny_limit[i]); pc.print("-"); pc.print(horny_limit[i]); pc.print(" "); }
 +
  pc.println("ok");
 +
  pipni();
 +
}
 +
 +
void vypis_kalibraciu()
 +
{
 +
  pc.print("stredy: ");
 +
  for (int i = 0; i < 6; i++) { pc.print(kalib[i]); pc.print(" "); }
 +
  pc.println();
 +
  pc.print("dolny limit: ");
 +
  for (int i = 0; i < 6; i++) { pc.print(dolny_limit[i]); pc.print(" "); }
 +
  pc.println();
 +
  pc.print("horny limit: ");
 +
  for (int i = 0; i < 6; i++) { pc.print(horny_limit[i]); pc.print(" "); }
 +
  pc.println();
 
}
 
}
  
 
void nacitaj_kalibraciu()
 
void nacitaj_kalibraciu()
 
{
 
{
  ch_len = 0;
 
 
   int tm;
 
   int tm;
 
   for (int i = 0; i < 6; i++)  
 
   for (int i = 0; i < 6; i++)  
 
     kalib[i] = nacitajCislo();
 
     kalib[i] = nacitajCislo();
   for (int i = 0; i < 6; i++) { pc.print(kalib[i]); pc.print(" "); }
+
   vypis_kalibraciu();
 
   pc.println("ok");
 
   pc.println("ok");
 
   pipni();
 
   pipni();
Line 349: Line 472:
 
     else if (z == ' ') reset();
 
     else if (z == ' ') reset();
 
     else if (z == 'H') kalibruj();
 
     else if (z == 'H') kalibruj();
 +
    else if (z == 'J') nastav_limity();
 +
    else if (z == 'G') vypis_kalibraciu();
 
     else if (z == 'L') nacitaj_kalibraciu();
 
     else if (z == 'L') nacitaj_kalibraciu();
 +
    else if (z == 'E') zapis_kalibraciu_do_EEPROM();
 +
    else if (z == 'R') ruky();
 
     else if (z == '9') zvys_krok();
 
     else if (z == '9') zvys_krok();
 
     else if (z == '1') zniz_krok();
 
     else if (z == '1') zniz_krok();

Latest revision as of 15:42, 2 August 2018

Program umožňuje priame riadenie robota (aj cez BlueTooth). Zatiaľ musíme pripájať BT na piny 0 a 1 (namiesto 2 a 4) a počas programovania vytiahnuť káblik označený TXD na strane BT modulu.

Na komunikáciu s robotom môžete použiť napr. program Putty: putty.exe.

klaves servo pin
a/q ľavá ruka 10
;/p pravá ruka 11
z/x ľavá noha 9
,/. pravá noha 6
d/c ľavá päta 5
k/m pravá päta 3
1/9 zníž/zvýš rýchlosť pohybu -
Tx BT 4
Rx BT 2
TRIG UZ 7
ECHO UZ 8
Sirena 12

Ďalej umožňuje

  • kalibrovať strednú polohu servomotorov (pomocou klávesu H)
    • pomocou + a - najdeme nulovu polohu serva
    • eneter ideme na dalsie servo
    • ukoncime E ulozenie do eprom
    • potvrdime Y
  • kalibrovať limity pre všetky stupne voľnosti (pomocou klávesu J), zobrazenie celej kalibrácie: kláves G
  • celú kalibráciu je možné zapísať do trvalej pamäte EEPROM pomocou klávesu E (používať opatrne!), po zapnutí sa automaticky načíta
  • pozdraví po klávese -, ruky dá hore na kláves R
  • načíta choreografiu po klávese @,
  • zobrazí načítanú choreografiu na kláves ? a
  • zatancuje načítanú choreografiu - kláves t.
  • Na klávesách 3-6 je možné nastaviť rôznu kombináciu (funkcie kombinacia1() - kombinacia4()).
  • Medzera resetne všetky servo motory do strednej polohy.

Pre komunikáciu na pinoch 0,1 pomocou bežného sériového portu treba pridať riadok #define pc Serial a zakomentovať riadky pc.begin(...); a SoftwareSerial pc(...)

  1 //#include <SoftwareSerial.h>
  2 #include <Servo.h>
  3 #include <EEPROM.h>
  4 
  5 #define pc Serial
  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 //SoftwareSerial pc(BT_TX, BT_RX);
 67 
 68 uint16_t ch_time[CHOREO_LEN];
 69 uint8_t ch_servo[CHOREO_LEN];
 70 uint8_t ch_val[CHOREO_LEN];
 71 int ch_len;
 72 uint8_t kalib[6];
 73 int stav[6];
 74 int krok;
 75 
 76 void setup() {
 77   randomSeed(analogRead(1));
 78   s[0].attach(PIN_SERVO_PRAVA_PATA);
 79   s[1].attach(PIN_SERVO_LAVA_PATA);
 80   s[2].attach(PIN_SERVO_PRAVA_NOHA);
 81   s[3].attach(PIN_SERVO_LAVA_NOHA);
 82   s[4].attach(PIN_SERVO_PRAVA_RUKA);
 83   s[5].attach(PIN_SERVO_LAVA_RUKA);
 84   precitaj_kalibraciu_z_EEPROM();
 85   for (int i = 0; i < 6; i++)
 86   {
 87     kalib[i] = prednastavena_kalibracia[i];
 88     stav[i] = kalib[i];
 89     s[i].write(stav[i]);
 90   }
 91   // pc.begin(9600);
 92   Serial.begin(9600);
 93   ch_len = 0;
 94   krok = 7;
 95   ahoj();
 96   ruky();
 97 }
 98 
 99 void precitaj_kalibraciu_z_EEPROM()
100 {
101   uint8_t value = EEPROM.read(1);
102   if (value != '~') return;
103   for (int i = 2; i < 8; i++)
104     prednastavena_kalibracia[i - 2] = EEPROM.read(i);
105   for (int i = 0; i < 6; i++)
106     dolny_limit[i] = EEPROM.read(i + 9);
107   for (int i = 0; i < 6; i++)
108     horny_limit[i] = EEPROM.read(i + 15);    
109 }
110 
111 void zapis_kalibraciu_do_EEPROM()
112 {
113   pc.print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
114   while (!pc.available());
115   char odpoved = pc.read();
116   pc.println(odpoved);
117   if (odpoved == 'Y') 
118   {
119     EEPROM.write(1, '~');
120     for (int i = 2; i < 8; i++)
121       EEPROM.write(i, kalib[i - 2]);
122     for (int i = 0; i < 6; i++)
123       EEPROM.write(9 + i, dolny_limit[i]);
124     for (int i = 0; i < 6; i++)
125       EEPROM.write(15 + i, horny_limit[i]);
126     pc.println("ok");
127   }
128 }
129 
130 void pipni()
131 {
132   tone(SIRENA, 1568, 50);
133   delay(100);
134   tone(SIRENA, 1357, 50);
135 }
136 
137 void ruky()
138 {
139   int odloz_krok = krok;
140   delay(500);
141   krok = 90;
142   pohyb(SERVO_LAVA_RUKA);    
143   pohyb(SERVO_PRAVA_RUKA);
144   delay(1000);
145   krok = 180;
146   pohyb(-SERVO_LAVA_RUKA);    
147   pohyb(-SERVO_PRAVA_RUKA);
148   delay(1000);
149   krok = odloz_krok;
150   pipni();
151 }
152 
153 void ahoj()
154 {
155   tone(SIRENA, 1568, 50);
156   delay(70);
157   tone(SIRENA, 1175, 30);
158   delay(50);
159   tone(SIRENA, 880, 30);
160   delay(50);
161   tone(SIRENA, 1047, 50);
162   delay(70);
163   tone(SIRENA, 1245, 30);
164   delay(150);
165   tone(SIRENA, 1568, 50);
166   delay(100);
167   if (random(10) > 4) tone(SIRENA, 1357, 50);
168   else tone(SIRENA, 1047, 50);
169 }
170 
171 void nastav_koncatinu(int8_t servo, uint8_t poloha)
172 {
173   int8_t srv = (servo > 0)?servo:-servo;
174   srv--;
175   poloha += kalib[srv] - 90;
176   if (poloha > 180) poloha = 180;
177   if (poloha < 0) poloha = 0;
178   stav[srv] = poloha;
179   s[srv].write(stav[srv]);
180 }
181 
182 void pohyb(int8_t servo)
183 {
184   int8_t srv = (servo > 0)?servo:-servo;
185   srv--;
186   if (servo_invertovane[srv]) servo = -servo;
187   if (servo > 0)
188   {
189     if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
190     else stav[srv] = horny_limit[srv];
191     s[srv].write(stav[srv]);
192   }
193   else if (servo < 0)
194   {
195     if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok; 
196     else stav[srv] = dolny_limit[srv];
197     s[srv].write(stav[srv]);      
198   }
199 }
200 
201 uint8_t pohyb_znakom(char z)
202 {
203   for (int i = 0; i < 12; i++)
204   {
205     if (z == znaky_zmien[i])
206     {
207       int8_t servo = zmeny[i];
208       pohyb(servo);
209     }
210   }
211 }
212 
213 void kombinacia1()
214 {
215   pohyb(SERVO_LAVA_NOHA);
216   pohyb(-SERVO_PRAVA_PATA);
217 }
218 
219 void kombinacia2()
220 {
221   pohyb(SERVO_PRAVA_NOHA);
222   pohyb(-SERVO_LAVA_PATA);
223 }
224 
225 void kombinacia3()
226 {
227    pohyb(SERVO_LAVA_RUKA); 
228    pohyb(SERVO_PRAVA_RUKA); 
229 }
230 
231 void kombinacia4()
232 {
233    pohyb(-SERVO_LAVA_RUKA); 
234    pohyb(-SERVO_PRAVA_RUKA); 
235 }
236 
237 int pohyb_kombinacia(char z)
238 {
239   if (z == '3') kombinacia1();
240   else if (z == '4') kombinacia2();
241   else if (z == '5') kombinacia3();
242   else if (z == '6') kombinacia4();
243   else return 0;
244   return 1;
245 }
246 
247 int nacitajCislo()
248 {
249   int num = 0;
250   int z;
251   do {
252     z = pc.read();
253     if (z == '#') while (z != 13) z = pc.read();
254   } while ((z < '0') || (z > '9'));
255   while ((z >= '0') && (z <= '9'))
256   {
257     num *= 10;
258     num += (z - '0');
259     do { z = pc.read(); if (z == -1) delayMicroseconds(10); } while (z < 0);
260   }
261   return num;
262 }
263 
264 void nacitaj_choreografiu()
265 {
266   ch_len = 0;
267   int tm;
268   do { 
269     tm = nacitajCislo();
270     ch_time[ch_len] = tm;
271     ch_servo[ch_len] = nacitajCislo();
272     ch_val[ch_len] = nacitajCislo();
273     ch_len++;
274   if (ch_len == CHOREO_LEN) break;
275   } while (tm > 0);
276   pipni();  
277 }
278 
279 void vypis_choreografiu()
280 {
281   for (int i = 0; i < ch_len; i++)
282   {
283     pc.print(ch_time[i]);
284     pc.print(" ");
285     pc.print(ch_servo[i]);
286     pc.print(" ");
287     pc.println(ch_val[i]);      
288   }
289   pipni();
290 }
291 
292 void zatancuj_choreografiu()
293 {
294   for (int i = 0; i < ch_len; i++)
295   {
296     delay(ch_time[i]);
297     nastav_koncatinu(ch_servo[i], ch_val[i]);
298   }
299   if (ch_len > 0) stav[ch_servo[ch_len - 1]] = ch_val[ch_len - 1];
300   pipni();
301 }
302 
303 void reset()
304 {
305   for (int i = 0; i < 6; i++) 
306   {
307     stav[i] = kalib[i];
308     s[i].write(kalib[i]);
309   }
310   pipni();
311 }
312 
313 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
314 {
315     pc.print(" (+/-/ENTER): ");
316     pc.println(hodnota);
317     s[servo].write(hodnota);
318     char z;
319     do {
320       z = pc.read();
321       if ((z == '+') && (hodnota < 180)) hodnota++;
322       else if ((z == '-') && (hodnota > 0)) hodnota--; 
323       if ((z == '+') || (z == '-'))
324       {
325         pc.print(hodnota); pc.print('\r'); 
326         s[servo].write(hodnota);
327       }
328     } while (z != 13);
329     return hodnota;
330 }
331 
332 void kalibruj()
333 {
334   for (int i = 0; i < 6; i++)
335   {
336     pc.print(i);
337     kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
338     pc.print(i);
339     pc.print(": ");
340     pc.println(kalib[i]);
341   }
342   for (int i = 0; i < 6; i++) { pc.print(kalib[i]); pc.print(" "); }
343   pc.println("ok");
344   pipni();
345 }
346 
347 void nastav_limity()
348 {
349   for (int i = 0; i < 6; i++)
350   {
351     pc.print(i);
352     pc.print("dolny");
353     dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
354     pc.print(i);
355     pc.print(" dolny: ");
356     pc.println(dolny_limit[i]);
357     s[i].write(kalib[i]);
358 
359     pc.print(i);
360     pc.print("horny");
361     horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
362     pc.print(i);
363     pc.print(" horny: ");
364     pc.println(horny_limit[i]);
365     s[i].write(kalib[i]);
366   }
367   for (int i = 0; i < 6; i++) { pc.print(dolny_limit[i]); pc.print("-"); pc.print(horny_limit[i]); pc.print(" "); }
368   pc.println("ok");
369   pipni();
370 }
371 
372 void vypis_kalibraciu()
373 {
374   pc.print("stredy: ");
375   for (int i = 0; i < 6; i++) { pc.print(kalib[i]); pc.print(" "); }
376   pc.println();
377   pc.print("dolny limit: ");
378   for (int i = 0; i < 6; i++) { pc.print(dolny_limit[i]); pc.print(" "); }
379   pc.println();
380   pc.print("horny limit: ");
381   for (int i = 0; i < 6; i++) { pc.print(horny_limit[i]); pc.print(" "); }
382   pc.println();
383 }
384 
385 void nacitaj_kalibraciu()
386 {
387   int tm;
388   for (int i = 0; i < 6; i++) 
389     kalib[i] = nacitajCislo();
390   vypis_kalibraciu();
391   pc.println("ok");
392   pipni();
393 }
394 
395 void zvys_krok()
396 {
397   if (krok < 180) krok++;
398   pc.print("krok: ");
399   pc.println(krok);
400 }
401 
402 void zniz_krok()
403 {
404   if (krok > 0) krok--;
405   pc.print("krok: ");
406   pc.println(krok);
407 }
408 
409 void loop() {
410   if (pc.available())
411   { 
412     char z = pc.read();
413     if (pohyb_znakom(z)) return;   
414     else if (pohyb_kombinacia(z)) return; 
415     else if (z == '@') nacitaj_choreografiu();
416     else if (z == '?') vypis_choreografiu();
417     else if (z == 't') zatancuj_choreografiu();
418     else if (z == '-') ahoj();
419     else if (z == ' ') reset();
420     else if (z == 'H') kalibruj();
421     else if (z == 'J') nastav_limity();
422     else if (z == 'G') vypis_kalibraciu();
423     else if (z == 'L') nacitaj_kalibraciu();
424     else if (z == 'E') zapis_kalibraciu_do_EEPROM();
425     else if (z == 'R') ruky();
426     else if (z == '9') zvys_krok();
427     else if (z == '1') zniz_krok();
428   }
429 }