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

From DT^2
Jump to: navigation, search
 
(9 intermediate revisions by 3 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 27: Line 32:
 
|}   
 
|}   
  
−
Ďalej umožňuje kalibrovať strednú polohu servomotorov (pomocou klávesu H),
+
Ďalej umožňuje  
−
pozdraví po klávese -, načíta choreografiu po klávese @, zobrazí načítanú choreografiu na kláves ? a zatancuje načítanú choreografiu - kláves t.
+
*kalibrovať strednú polohu servomotorov (pomocou klávesu '''H''')
−
Na klávesách 3-6 je možné nastaviť rôznu kombináciu (funkcie kombinacia1() - kombinacia4()).
+
** pomocou + a - najdeme nulovu polohu serva
−
Medzera resetne všetky servo motory do strednej polohy.
+
** 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
 
''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 46: 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 71: 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 79: 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 95: 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 107: 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 154: 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 163: 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 248: 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 270: 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 285: 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 306: 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 345: 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 }
Retrieved from "https://dtdt.fablab.sk/w/index.php?title=Otto_-_riadiaci_program_v.1&oldid=133"