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

From DT^2
Jump to: navigation, search
 
(9 intermediate revisions by 3 users not shown)
Line 5: Line 5:
  
 
{|class="wikitable"
 
{|class="wikitable"
!klaves!!servo!!pin!!Kalibracia
+
!klaves!!servo!!pin!!číslo serva v choreografii
 
|-
 
|-
|a/q || ľavá ruka ||  10 ||4
+
|a/q || ľavá ruka ||  10 ||5
 
|-
 
|-
|;/p || pravá ruka||  11 ||5
+
|;/p || pravá ruka||  11 ||6
 
|-
 
|-
|z/x || ľavá noha  || 9 ||3
+
|z/x || ľavá noha  || 9 ||4
 
|-
 
|-
|,/. || pravá noha || 6 ||2
+
|,/. || pravá noha || 6 ||3
 
|-
 
|-
|d/c || ľavá päta  || 5 ||1
+
|d/c || ľavá päta  || 5 ||2
 
|-
 
|-
|k/m || pravá päta || 3 ||0
+
|k/m || pravá päta || 3 ||1
 
|-
 
|-
 
|1/9 || zníž/zvýš rýchlosť pohybu || -
 
|1/9 || zníž/zvýš rýchlosť pohybu || -
Line 34: Line 34:
 
Ďalej umožňuje  
 
Ďalej umožňuje  
 
*kalibrovať strednú polohu servomotorov (pomocou klávesu '''H''')
 
*kalibrovať strednú polohu servomotorov (pomocou klávesu '''H''')
** pomocou + a - najdeme nulovu polohu serva
+
** pomocou + a - nájdeme strednú polohu serva
** eneter ideme na dalsie servo
+
** enter potvrdí hodnotu a pokračuje sa ďalším servom
** ukoncime E ulozenie do eprom
+
** kalibrácia sa po vypnutí nezapamätá, pokiaľ ju neuložíte do EEPROM (pozri nižšie)
** potvrdime Y
 
 
*kalibrovať limity pre všetky stupne voľnosti (pomocou klávesu '''J'''), zobrazenie celej kalibrácie: kláves '''G'''
 
*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
+
*celú kalibráciu je možné zapísať do trvalej pamäte EEPROM pomocou klávesu '''E''' (používať opatrne! - dá sa prepísať najviac 100000-krát), po zapnutí sa automaticky načíta
 
*pozdraví po klávese '''-''', ruky dá hore na kláves '''R'''
 
*pozdraví po klávese '''-''', ruky dá hore na kláves '''R'''
 
*načíta choreografiu po klávese '''@''',  
 
*načíta choreografiu po klávese '''@''',  
Line 47: Line 46:
 
*Kláves '''U''' testuje ultrazvuk
 
*Kláves '''U''' testuje ultrazvuk
 
*'''Medzera''' resetne všetky servo motory do strednej polohy.
 
*'''Medzera''' resetne všetky servo motory do strednej polohy.
 +
 +
Pozrite si (a skopírujte si) príklady choreografií: [[Otto - príklady choreografií]]
  
 
<syntaxhighlight lang="C" line=line>
 
<syntaxhighlight lang="C" line=line>
Line 162: Line 163:
 
     else if (z == '@') nacitaj_choreografiu();
 
     else if (z == '@') nacitaj_choreografiu();
 
     else if (z == '?') vypis_choreografiu();
 
     else if (z == '?') vypis_choreografiu();
     else if (z == 't') zatancuj_choreografiu();
+
     else if (z == 't') zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
 
     else if (z == '-') ahoj();
 
     else if (z == '-') ahoj();
 
     else if (z == ' ') reset();
 
     else if (z == ' ') reset();
Line 194: Line 195:
 
   uint32_t tm = millis();
 
   uint32_t tm = millis();
 
   int d = measure_distance();
 
   int d = measure_distance();
   while ((millis() - tm < 3000) && (d < 15))  
+
  int count;
 +
   while ((millis() - tm < 3000) && (d < 15) & (count < 10))  
 
   {
 
   {
 
     delay(10);
 
     delay(10);
 
     d = measure_distance();
 
     d = measure_distance();
 +
    if (d == 10000) count++;
 +
    else count = 0;
 
   }
 
   }
   if (d < 15)
+
   if (millis() - tm >= 3000)
 
     ultrasonic_menu();
 
     ultrasonic_menu();
 
}
 
}
Line 240: Line 244:
 
void menu_command(int cmd)
 
void menu_command(int cmd)
 
{
 
{
 +
  if (cmd == 2) ahoj();
 +
  if (cmd == 3) melodia();
 +
  if (cmd == 1) vpred();
 
   serial_println_num(cmd);
 
   serial_println_num(cmd);
 +
}
 +
 +
void melodia()
 +
{
 +
  for (int i = 0; i < 2; i++)
 +
  {
 +
    tone(SIRENA, 262, 200);
 +
    delay(200);
 +
    tone(SIRENA, 330, 200);
 +
    delay(200);
 +
 
 +
    tone(SIRENA, 262, 200);
 +
    delay(200);
 +
    tone(SIRENA, 330, 200);
 +
    delay(200);
 +
 
 +
    tone(SIRENA, 392, 400);
 +
    delay(400);
 +
 
 +
    tone(SIRENA, 392, 400);
 +
    delay(400);
 +
  }
 +
  noTone(8);
 +
}
 +
 +
 +
uint16_t chor_time[] = { 500, 500,  1, 500, 500, 500,  1, 500,  1, 500, 500,  1, 500,  1, 0 };
 +
uint8_t chor_servo[] = {  5,  2,  1,  3,  4,  2,  1,  1,  2,  2,  4,  3,  1,  2, 0 };
 +
uint8_t chor_val[] =  {  10, 115,  40, 135, 135,  90,  90, 115,  40,  90,  90,  90,  90,  90, 0 };
 +
uint8_t chor_len = 15;
 +
 +
void vpred()
 +
{
 +
  while (measure_distance() > 30)   
 +
    zatancuj_choreografiu(chor_time, chor_servo, chor_val, chor_len);
 +
  pipni();
 
}
 
}
  
Line 418: Line 461:
 
   int z;
 
   int z;
 
   do {
 
   do {
     z = serial_read();
+
     z = read_char();
     if (z == '#') while (z != 13) z = serial_read();
+
     if (z == '#') while (z != 13) z = read_char();
 
   } while ((z < '0') || (z > '9'));
 
   } while ((z < '0') || (z > '9'));
 
   while ((z >= '0') && (z <= '9'))
 
   while ((z >= '0') && (z <= '9'))
Line 425: Line 468:
 
     num *= 10;
 
     num *= 10;
 
     num += (z - '0');
 
     num += (z - '0');
     do { z = serial_read(); if (z == -1) delayMicroseconds(10); } while (z < 0);
+
     do { z = read_char(); if (z == -1) delayMicroseconds(10); } while (z < 0);
 
   }
 
   }
 
   return num;
 
   return num;
Line 458: Line 501:
 
}
 
}
  
void zatancuj_choreografiu()
+
void zatancuj_choreografiu(uint16_t *ch_time, uint8_t *ch_servo, uint8_t *ch_val, int ch_len )
 
{
 
{
   for (int i = 0; i < ch_len; i++)
+
   for (int i = 0; i < ch_len - 1; i++)
 
   {
 
   {
 
     delay(ch_time[i]);
 
     delay(ch_time[i]);
Line 467: Line 510:
 
   if (ch_len > 0) stav[ch_servo[ch_len - 1]] = ch_val[ch_len - 1];
 
   if (ch_len > 0) stav[ch_servo[ch_len - 1]] = ch_val[ch_len - 1];
 
   pipni();
 
   pipni();
 +
  while (serial_available()) serial_read();
 +
  while (Serial.available()) Serial.read();
 
}
 
}
  
Line 475: Line 520:
 
     stav[i] = kalib[i];
 
     stav[i] = kalib[i];
 
     s[i].write(kalib[i]);
 
     s[i].write(kalib[i]);
 +
    delay(300);
 
   }
 
   }
 
   pipni();
 
   pipni();
Line 858: Line 904:
 
   wait_for_distance_measurement_to_complete();
 
   wait_for_distance_measurement_to_complete();
 
   return distance;
 
   return distance;
}
+
}</syntaxhighlight>
</syntaxhighlight>
+
V programe puTTy môžete použiť aj tento jednoduchý robotí tanec
 +
<pre>
 +
@ 1000 1 60
 +
1 5 90
 +
1 6 90
 +
1000 1 90
 +
1 5 120
 +
1000 1 120
 +
1 6 120
 +
1 6 90
 +
1 2 120
 +
1 5 90
 +
100  1 90
 +
1 5 45 
 +
100  2 90
 +
1 6 45
 +
1000 2 60
 +
1 5 120
 +
1000 2 90
 +
1 6 120
 +
1000 2 120
 +
1 1 120
 +
100 2 90
 +
1 6 90
 +
100 1 90
 +
1 5 90
 +
0 0 0
 +
</pre>
 +
ak toto vložíte do textove dokumentu a zkopírujete, pravým kliknitímna plochu v puTTy a následným stlačením tlačidla T váš robot začne tancovať

Latest revision as of 17:46, 8 August 2018

Program umožňuje priame riadenie robota (aj cez BlueTooth). Teraz už funguje aj na pinoch 2,4 a môže sa naraz programovať aj komunikovať.

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

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

Ďalej umožňuje

  • kalibrovať strednú polohu servomotorov (pomocou klávesu H)
    • pomocou + a - 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)
  • 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! - dá sa prepísať najviac 100000-krát), po zapnutí sa automaticky načíta
  • pozdraví po klávese -, ruky dá hore na kláves R
  • načíta choreografiu po klávese @,
  • zobrazí načítanú choreografiu na kláves ? a
  • zatancuje načítanú choreografiu - kláves t.
  • Na klávesách 3-6 je možné nastaviť rôznu kombináciu (funkcie kombinacia1() - kombinacia4()).
  • Kláves U testuje ultrazvuk
  • Medzera resetne všetky servo motory do strednej polohy.

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

  1 #include <Servo.h>
  2 #include <EEPROM.h>
  3 
  4 #define ECHO_BT_TO_USB 1
  5 
  6 #define US_TRIG  7
  7 #define US_ECHO  8
  8 
  9 #define BT_RX   2 
 10 #define BT_TX   4
 11 
 12 #define SIRENA 12
 13 
 14 //maximalna dlzka choreografie
 15 #define CHOREO_LEN 200
 16 
 17 // cisla pinov, kde su zapojene servo motory
 18 #define PIN_SERVO_LAVA_RUKA   10 
 19 #define PIN_SERVO_PRAVA_RUKA  11
 20 #define PIN_SERVO_LAVA_NOHA   9
 21 #define PIN_SERVO_PRAVA_NOHA  6
 22 #define PIN_SERVO_LAVA_PATA   5
 23 #define PIN_SERVO_PRAVA_PATA  3
 24 
 25 #define S1 3
 26 #define S2 5
 27 #define S3 6
 28 #define S4 9
 29 #define S5 10
 30 #define S6 11
 31 #define SIRENA 12
 32 
 33 //maximalna dlzka choreografie
 34 #define CHOREO_LEN 200
 35 
 36 // tu su serva cislovane 1-6
 37 #define SERVO_LAVA_RUKA   5 
 38 #define SERVO_PRAVA_RUKA  6
 39 #define SERVO_LAVA_NOHA   4
 40 #define SERVO_PRAVA_NOHA  3
 41 #define SERVO_LAVA_PATA   2
 42 #define SERVO_PRAVA_PATA  1
 43 
 44 // ak su niektore serva naopak, je tu jednotka
 45 uint8_t servo_invertovane[6] = {0, 0, 1, 1, 0, 1};
 46 
 47 // znaky, ktorymi sa ovladaju jednotlive stupne volnosti
 48 char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' };
 49 // co robia jednotlive znaky (znamienko urcuje smer)
 50 int8_t zmeny[] = {SERVO_LAVA_RUKA, -SERVO_LAVA_RUKA, 
 51                   SERVO_PRAVA_RUKA, -SERVO_PRAVA_RUKA, 
 52                   -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA, 
 53                   -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA, 
 54                    SERVO_LAVA_PATA, -SERVO_LAVA_PATA, 
 55                    SERVO_PRAVA_PATA, -SERVO_PRAVA_PATA }; 
 56 
 57 // sem si mozno ulozit svoju kalibraciu
 58 //uint8_t prednastavena_kalibracia[] = { 78, 69, 83, 80, 50, 67 };
 59 uint8_t prednastavena_kalibracia[] = { 90, 90, 90, 90, 90, 90 };
 60 
 61 uint8_t dolny_limit[] = {0, 0, 0, 0, 0, 0, 0, 0};
 62 uint8_t horny_limit[] = {180, 180, 180, 180, 180, 180};
 63 
 64 Servo s[6];
 65 
 66 uint16_t ch_time[CHOREO_LEN];
 67 uint8_t ch_servo[CHOREO_LEN];
 68 uint8_t ch_val[CHOREO_LEN];
 69 int ch_len;
 70 uint8_t kalib[6];
 71 int stav[6];
 72 int krok;
 73 
 74 void setup() {
 75   Serial.begin(9600);
 76   init_serial(9600);
 77   init_ultrasonic();
 78 
 79   randomSeed(analogRead(1));
 80   s[0].attach(PIN_SERVO_PRAVA_PATA);
 81   s[1].attach(PIN_SERVO_LAVA_PATA);
 82   s[2].attach(PIN_SERVO_PRAVA_NOHA);
 83   s[3].attach(PIN_SERVO_LAVA_NOHA);
 84   s[4].attach(PIN_SERVO_PRAVA_RUKA);
 85   s[5].attach(PIN_SERVO_LAVA_RUKA);
 86   precitaj_kalibraciu_z_EEPROM();
 87   for (int i = 0; i < 6; i++)
 88   {
 89     kalib[i] = prednastavena_kalibracia[i];
 90     stav[i] = kalib[i];
 91     s[i].write(stav[i]);
 92   }
 93   ch_len = 0;
 94   krok = 7;
 95   ahoj();
 96   ruky2();
 97   delay(100);  
 98   serial_println("\r\n  Otto DTDT");
 99 }
100 
101 void loop() {
102   char z = -1;
103   if (serial_available()) z = serial_read();
104 #ifdef ECHO_BT_TO_USB
105     if (Serial.available()) z = Serial.read();
106 #endif  
107   
108   if (z != -1)
109   { 
110     if (pohyb_znakom(z)) return;   
111     else if (pohyb_kombinacia(z)) return; 
112     else if (z == '@') nacitaj_choreografiu();
113     else if (z == '?') vypis_choreografiu();
114     else if (z == 't') zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
115     else if (z == '-') ahoj();
116     else if (z == ' ') reset();
117     else if (z == 'H') kalibruj();
118     else if (z == 'J') nastav_limity();
119     else if (z == 'G') vypis_kalibraciu();
120     else if (z == 'L') nacitaj_kalibraciu();
121     else if (z == 'E') zapis_kalibraciu_do_EEPROM();
122     else if (z == 'R') ruky();
123     else if (z == '9') zvys_krok();
124     else if (z == '1') zniz_krok();
125     else if (z == 'U') test_ultrazvuk();
126   }
127   int16_t d = measure_distance();
128   if (d < 10) menu_ultrasonic_request();
129 }
130 
131 void test_ultrazvuk()
132 {
133   int i = 0;
134   while ((serial_available() == 0) && (Serial.available() == 0))
135   {
136     serial_println_num(measure_distance());
137     delay(100);
138   }
139   serial_read();
140 }
141 
142 void menu_ultrasonic_request()
143 {
144   uint32_t tm = millis();
145   int d = measure_distance();
146   int count;
147   while ((millis() - tm < 3000) && (d < 15) & (count < 10)) 
148   {
149     delay(10);
150     d = measure_distance();
151     if (d == 10000) count++;
152     else count = 0;
153   }
154   if (millis() - tm >= 3000)
155     ultrasonic_menu();
156 }
157 
158 void ultrasonic_menu()
159 {
160   int selection = 0;
161   tone(SIRENA, 880, 200);
162   
163   do {  
164     int count = 0;
165     do { 
166       int32_t d = measure_distance();
167       if (d == 10000) continue;
168       if (d >= 20) count++;
169       else count = 0;
170       delay(10);
171     } while (!serial_available() && !Serial.available() && (count < 20));
172     
173     tone(SIRENA, 440, 200);
174     uint32_t tm = millis();
175     while ((measure_distance() > 15) && (millis() - tm < 1500) && !serial_available() && !Serial.available()) delay(10);
176     if (millis() - tm >= 1500) 
177     {
178       tone(SIRENA, 2000, 50);
179       menu_command(selection);
180       return;
181     }       
182     selection++;
183     for (int i = 0; i < selection; i++)
184     {
185       tone(SIRENA, 1261, 50);
186       delay(250);
187     }
188   } while (!serial_available() && !Serial.available());
189   while (serial_available()) serial_read();
190   while (Serial.available()) Serial.read();
191 }
192 
193 void menu_command(int cmd)
194 {
195   if (cmd == 2) ahoj();
196   if (cmd == 3) melodia();
197   if (cmd == 1) vpred();
198   serial_println_num(cmd);
199 }
200 
201 void melodia()
202 {
203   for (int i = 0; i < 2; i++)
204   {
205     tone(SIRENA, 262, 200);
206     delay(200);
207     tone(SIRENA, 330, 200);
208     delay(200);
209   
210     tone(SIRENA, 262, 200);
211     delay(200);
212     tone(SIRENA, 330, 200);
213     delay(200);
214   
215     tone(SIRENA, 392, 400);
216     delay(400);
217   
218     tone(SIRENA, 392, 400);
219     delay(400);
220   }
221   noTone(8);
222 }
223 
224 
225 uint16_t chor_time[] = { 500, 500,   1, 500, 500, 500,   1, 500,   1, 500, 500,   1, 500,   1, 0 };
226 uint8_t chor_servo[] = {   5,   2,   1,   3,   4,   2,   1,   1,   2,   2,   4,   3,   1,   2, 0 };
227 uint8_t chor_val[] =   {  10, 115,  40, 135, 135,  90,  90, 115,  40,  90,  90,  90,  90,  90, 0 };
228 uint8_t chor_len = 15;
229 
230 void vpred()
231 {
232   while (measure_distance() > 30)    
233     zatancuj_choreografiu(chor_time, chor_servo, chor_val, chor_len);
234   pipni();
235 }
236 
237 void precitaj_kalibraciu_z_EEPROM()
238 {
239   uint8_t value = EEPROM.read(1);
240   if (value != '~') return;
241   for (int i = 2; i < 8; i++)
242     prednastavena_kalibracia[i - 2] = EEPROM.read(i);
243   for (int i = 0; i < 6; i++)
244     dolny_limit[i] = EEPROM.read(i + 9);
245   for (int i = 0; i < 6; i++)
246     horny_limit[i] = EEPROM.read(i + 15);    
247 }
248 
249 char read_char()
250 {
251   while (!serial_available() && !Serial.available());
252   if (serial_available()) return serial_read();
253   else return Serial.read();
254 }
255 
256 void zapis_kalibraciu_do_EEPROM()
257 {
258   serial_print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
259   char odpoved = read_char();
260   serial_println_char(odpoved);
261   if (odpoved == 'Y') 
262   {
263     EEPROM.write(1, '~');
264     for (int i = 2; i < 8; i++)
265       EEPROM.write(i, kalib[i - 2]);
266     for (int i = 0; i < 6; i++)
267       EEPROM.write(9 + i, dolny_limit[i]);
268     for (int i = 0; i < 6; i++)
269       EEPROM.write(15 + i, horny_limit[i]);
270     serial_println("ok");
271   }
272 }
273 
274 void pipni()
275 {
276   tone(SIRENA, 1568, 50);
277   delay(100);
278   tone(SIRENA, 1357, 50);
279 }
280 
281 void ruky()
282 {
283   int odloz_krok = krok;
284   delay(500);
285   krok = 90;
286   pohyb(SERVO_LAVA_RUKA);    
287   pohyb(SERVO_PRAVA_RUKA);
288   delay(1000);
289   krok = 180;
290   pohyb(-SERVO_LAVA_RUKA);    
291   pohyb(-SERVO_PRAVA_RUKA);
292   delay(1000);
293   krok = odloz_krok;
294   pipni();
295 }
296 
297 void ruky2()
298 {
299   int odloz_krok = krok;
300   delay(500);
301   krok = 180;
302   pohyb(SERVO_LAVA_RUKA);    
303   pohyb(SERVO_PRAVA_RUKA);
304   delay(1000);
305   krok = 90;
306   pohyb(-SERVO_LAVA_RUKA);    
307   pohyb(-SERVO_PRAVA_RUKA);
308   delay(1000);
309   krok = odloz_krok;
310   pipni();
311 }
312 
313 void ahoj()
314 {
315   tone(SIRENA, 1568, 50);
316   delay(70);
317   tone(SIRENA, 1175, 30);
318   delay(50);
319   tone(SIRENA, 880, 30);
320   delay(50);
321   tone(SIRENA, 1047, 50);
322   delay(70);
323   tone(SIRENA, 1245, 30);
324   delay(150);
325   tone(SIRENA, 1568, 50);
326   delay(100);
327   if (random(10) > 4) tone(SIRENA, 1357, 50);
328   else tone(SIRENA, 1047, 50);
329 }
330 
331 void nastav_koncatinu(int8_t servo, uint8_t poloha)
332 {
333   int8_t srv = (servo > 0)?servo:-servo;
334   srv--;
335   poloha += kalib[srv] - 90;
336   if (poloha > 180) poloha = 180;
337   if (poloha < 0) poloha = 0;
338   stav[srv] = poloha;
339   s[srv].write(stav[srv]);
340 }
341 
342 void pohyb(int8_t servo)
343 {
344   int8_t srv = (servo > 0)?servo:-servo;
345   srv--;
346   if (servo_invertovane[srv]) servo = -servo;
347   if (servo > 0)
348   {
349     if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
350     else stav[srv] = horny_limit[srv];
351     s[srv].write(stav[srv]);
352   }
353   else if (servo < 0)
354   {
355     if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok; 
356     else stav[srv] = dolny_limit[srv];
357     s[srv].write(stav[srv]);      
358   }
359 }
360 
361 uint8_t pohyb_znakom(char z)
362 {
363   for (int i = 0; i < 12; i++)
364   {
365     if (z == znaky_zmien[i])
366     {
367       int8_t servo = zmeny[i];
368       pohyb(servo);
369     }
370   }
371 }
372 
373 void kombinacia1()
374 {
375   pohyb(SERVO_LAVA_NOHA);
376   pohyb(-SERVO_PRAVA_PATA);
377 }
378 
379 void kombinacia2()
380 {
381   pohyb(SERVO_PRAVA_NOHA);
382   pohyb(-SERVO_LAVA_PATA);
383 }
384 
385 void kombinacia3()
386 {
387    pohyb(SERVO_LAVA_RUKA); 
388    pohyb(SERVO_PRAVA_RUKA); 
389 }
390 
391 void kombinacia4()
392 {
393    pohyb(-SERVO_LAVA_RUKA); 
394    pohyb(-SERVO_PRAVA_RUKA); 
395 }
396 
397 int pohyb_kombinacia(char z)
398 {
399   if (z == '3') kombinacia1();
400   else if (z == '4') kombinacia2();
401   else if (z == '5') kombinacia3();
402   else if (z == '6') kombinacia4();
403   else return 0;
404   return 1;
405 }
406 
407 int nacitajCislo()
408 {
409   int num = 0;
410   int z;
411   do {
412     z = read_char();
413     if (z == '#') while (z != 13) z = read_char();
414   } while ((z < '0') || (z > '9'));
415   while ((z >= '0') && (z <= '9'))
416   {
417     num *= 10;
418     num += (z - '0');
419     do { z = read_char(); if (z == -1) delayMicroseconds(10); } while (z < 0);
420   }
421   return num;
422 }
423 
424 void nacitaj_choreografiu()
425 {
426   ch_len = 0;
427   int tm;
428   do { 
429     tm = nacitajCislo();
430     ch_time[ch_len] = tm;
431     ch_servo[ch_len] = nacitajCislo();
432     ch_val[ch_len] = nacitajCislo();
433     ch_len++;
434   if (ch_len == CHOREO_LEN) break;
435   } while (tm > 0);
436   pipni();  
437 }
438 
439 void vypis_choreografiu()
440 {
441   for (int i = 0; i < ch_len; i++)
442   {
443     serial_print_num(ch_time[i]);
444     serial_print(" ");
445     serial_print_num(ch_servo[i]);
446     serial_print(" ");
447     serial_println_num(ch_val[i]);      
448   }
449   pipni();
450 }
451 
452 void zatancuj_choreografiu(uint16_t *ch_time, uint8_t *ch_servo, uint8_t *ch_val, int ch_len )
453 {
454   for (int i = 0; i < ch_len - 1; i++)
455   {
456     delay(ch_time[i]);
457     nastav_koncatinu(ch_servo[i], ch_val[i]);
458   }
459   if (ch_len > 0) stav[ch_servo[ch_len - 1]] = ch_val[ch_len - 1];
460   pipni();
461   while (serial_available()) serial_read();
462   while (Serial.available()) Serial.read();
463 }
464 
465 void reset()
466 {
467   for (int i = 0; i < 6; i++) 
468   {
469     stav[i] = kalib[i];
470     s[i].write(kalib[i]);
471     delay(300);
472   }
473   pipni();
474 }
475 
476 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
477 {
478     serial_print(" (+/-/ENTER): ");
479     serial_println_num(hodnota);
480     s[servo].write(hodnota);
481     char z;
482     do {
483       z = read_char();
484       if ((z == '+') && (hodnota < 180)) hodnota++;
485       else if ((z == '-') && (hodnota > 0)) hodnota--; 
486       if ((z == '+') || (z == '-'))
487       {
488         serial_print_num(hodnota); serial_print_char('\r'); 
489         s[servo].write(hodnota);
490       }
491     } while (z != 13);
492     return hodnota;
493 }
494 
495 void kalibruj()
496 {
497   for (int i = 0; i < 6; i++)
498   {
499     serial_print_num(i);
500     kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
501     serial_print_num(i);
502     serial_print(": ");
503     serial_println_num(kalib[i]);
504   }
505   for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
506   serial_println("ok");
507   pipni();
508 }
509 
510 void nastav_limity()
511 {
512   for (int i = 0; i < 6; i++)
513   {
514     serial_print_num(i);
515     serial_print("dolny");
516     dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
517     serial_print_num(i);
518     serial_print(" dolny: ");
519     serial_println_num(dolny_limit[i]);
520     s[i].write(kalib[i]);
521 
522     serial_print_num(i);
523     serial_print("horny");
524     horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
525     serial_print_num(i);
526     serial_print(" horny: ");
527     serial_println_num(horny_limit[i]);
528     s[i].write(kalib[i]);
529   }
530   for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print("-"); serial_print_num(horny_limit[i]); serial_print(" "); }
531   serial_println("ok");
532   pipni();
533 }
534 
535 void vypis_kalibraciu()
536 {
537   serial_print("stredy: ");
538   for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
539   serial_println();
540   serial_print("dolny limit: ");
541   for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print(" "); }
542   serial_println();
543   serial_print("horny limit: ");
544   for (int i = 0; i < 6; i++) { serial_print_num(horny_limit[i]); serial_print(" "); }
545   serial_println();
546 }
547 
548 void nacitaj_kalibraciu()
549 {
550   int tm;
551   for (int i = 0; i < 6; i++) 
552     kalib[i] = nacitajCislo();
553   vypis_kalibraciu();
554   serial_println("ok");
555   pipni();
556 }
557 
558 void zvys_krok()
559 {
560   if (krok < 180) krok++;
561   serial_print("krok: ");
562   serial_println_num(krok);
563 }
564 
565 void zniz_krok()
566 {
567   if (krok > 0) krok--;
568   serial_print("krok: ");
569   serial_println_num(krok);
570 }
571 
572 // nasleduje softverova implementacia serioveho portu
573 #define SERIAL_STATE_IDLE      0
574 #define SERIAL_STATE_RECEIVING 1
575 #define SERIAL_BUFFER_LENGTH   20
576 
577 static volatile uint8_t serial_state;
578 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
579 static volatile uint8_t serial_buf_wp, serial_buf_rp;
580 
581 static volatile uint8_t receiving_byte;
582 
583 static volatile uint32_t time_startbit_noticed;
584 static volatile uint8_t next_bit_order;
585 static volatile uint8_t waiting_stop_bit;
586 static uint16_t one_byte_duration;
587 static uint16_t one_bit_duration;
588 static uint16_t one_bit_write_duration;
589 static uint16_t half_of_one_bit_duration;
590 
591 void init_serial(uint32_t baud_rate)
592 {
593   pinMode(2, INPUT);
594   pinMode(4, OUTPUT);
595   
596   serial_state = SERIAL_STATE_IDLE;
597   
598   one_byte_duration = 9500000 / baud_rate;
599   one_bit_duration = 1000000 / baud_rate;
600   one_bit_write_duration = one_bit_duration - 1;
601   half_of_one_bit_duration = 500000 / baud_rate;
602   
603   PCMSK2 |= 4; //PCINT18;
604   PCIFR &= ~4; //PCIF2;
605   PCICR |= 4; // PCIE2;
606 }
607 
608 ISR(PCINT2_vect)
609 {
610   uint32_t tm = micros();
611   if (serial_state == SERIAL_STATE_IDLE)
612   {    
613     time_startbit_noticed = tm;
614     serial_state = SERIAL_STATE_RECEIVING;
615     receiving_byte = 0xFF;
616     next_bit_order = 0;
617   }
618   else if (tm - time_startbit_noticed > one_byte_duration)
619   {
620       serial_buffer[serial_buf_wp] = receiving_byte;
621       serial_buf_wp++;
622       if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
623       time_startbit_noticed = tm;
624       receiving_byte = 0xFF;
625       next_bit_order = 0;
626   }
627   else if (PIND & 4) 
628   {
629      int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
630      while (next_bit_order < new_next_bit_order)
631      {  
632         receiving_byte &= ~(1 << next_bit_order);
633         next_bit_order++;
634      }
635      if (next_bit_order == 8)
636      { 
637         serial_buffer[serial_buf_wp] = receiving_byte;
638         serial_buf_wp++;
639         if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
640         serial_state = SERIAL_STATE_IDLE;
641      }        
642   } else 
643       next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
644 }
645 
646 uint8_t serial_available()
647 {
648   cli();
649   if (serial_buf_rp != serial_buf_wp) 
650   {
651     sei();
652     return 1;
653   }
654   if (serial_state == SERIAL_STATE_RECEIVING)
655   {
656     uint32_t tm = micros();
657     if (tm - time_startbit_noticed > one_byte_duration)
658     {      
659       serial_state = SERIAL_STATE_IDLE;
660       serial_buffer[serial_buf_wp] = receiving_byte;
661       serial_buf_wp++;
662       if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
663       sei();
664       return 1;
665     }
666   }
667   sei();
668   return 0;
669 }
670 
671 int16_t serial_read()
672 {
673   cli();
674   if (serial_buf_rp != serial_buf_wp)
675   {
676     uint8_t ch = serial_buffer[serial_buf_rp];
677     serial_buf_rp++;
678     if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
679     sei();
680     return ch;
681   }
682 
683   if (serial_state == SERIAL_STATE_RECEIVING)
684   {
685     uint32_t tm = micros();
686     if (tm - time_startbit_noticed > one_byte_duration)
687     {
688       uint8_t ch = receiving_byte;
689       serial_state = SERIAL_STATE_IDLE;  
690       sei();
691       return ch;
692     }
693   }
694   sei();
695   return -1;
696 }
697 
698 void serial_write(uint8_t ch)
699 {
700 #ifdef ECHO_BT_TO_USB
701   Serial.print((char)ch);
702 #endif
703   PORTD &= ~16;
704   delayMicroseconds(one_bit_write_duration);
705   for (uint8_t i = 0; i < 8; i++)
706   {
707     if (ch & 1) PORTD |= 16;
708     else PORTD &= ~16;
709     ch >>= 1;
710     delayMicroseconds(one_bit_write_duration);
711   }
712   PORTD |= 16;
713   delayMicroseconds(one_bit_write_duration);
714   delayMicroseconds(one_bit_write_duration);
715   delayMicroseconds(one_bit_write_duration);
716   delayMicroseconds(one_bit_write_duration);
717   delayMicroseconds(one_bit_write_duration);
718 }
719 
720 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
721 {
722   uint16_t len;
723   uint16_t ch;
724   do {
725     ch = serial_read();
726     if (ch == 13) continue;
727   } while (ch == -1);
728 
729   do {
730     if ((ch != 13) && (ch != 10) && (ch != -1)) 
731     {
732       *(ln++) = ch;
733       max_length--;
734       len++;
735     }    
736     ch = serial_read();
737   } while ((ch != 13) && max_length);
738   *ln = 0;
739   return len;
740 }
741 
742 void serial_print_num(int32_t number)
743 {
744   if (number < 0) 
745   {
746     serial_write('-');
747     number = -number;
748   }
749   int32_t rad = 1;
750   while (number / rad) rad *= 10;
751   if (number > 0) rad /= 10;
752   while (rad)
753   {
754     serial_write((char)('0' + (number / rad)));
755     number -= (number / rad) * rad;
756     rad /= 10;
757   }  
758 }
759 
760 void serial_print_char(char ch)
761 {
762   serial_write(ch);  
763 }
764 
765 void serial_print(const uint8_t *str)
766 {
767   while (*str) serial_write(*(str++));
768 }
769 
770 void serial_println(const uint8_t *str)
771 {
772   serial_print(str);
773   serial_write(13);
774   serial_write(10);
775 }
776 
777 void serial_println_num(int32_t number)
778 {
779   serial_print_num(number);
780   serial_println();
781 }
782 
783 void serial_println_char(char ch)
784 {
785   serial_write(ch);
786   serial_println();
787 }
788 
789 void serial_println()
790 {
791   serial_write(13);
792   serial_write(10);
793 }
794 
795 // nasleduje citanie z utltazvukoveho senzora
796 
797 static volatile uint32_t pulse_start;
798 static volatile int16_t distance;
799 static volatile uint8_t new_distance;
800 
801 void init_ultrasonic()
802 {
803   pinMode(US_ECHO, INPUT);
804   pinMode(US_TRIG, OUTPUT);
805   
806   PCMSK0 |= 1; //PCINT0;
807   PCIFR &= ~1; //PCIF0;
808   PCICR |= 1; // PCIE0;  
809 }
810 
811 ISR(PCINT0_vect)
812 {
813   if (PINB & 1) pulse_start = micros();
814   else 
815   {
816     distance = (int16_t)((micros() - pulse_start) / 58);
817     new_distance = 1;
818   }
819 }
820 
821 void start_distance_measurement()
822 {
823   distance = 10000;
824   new_distance = 0;
825   digitalWrite(US_TRIG, HIGH);
826   delayMicroseconds(10);
827   digitalWrite(US_TRIG, LOW);
828 }
829 
830 void wait_for_distance_measurement_to_complete()
831 {
832   uint8_t counter = 0;
833   while ((counter < 20) && !new_distance) 
834   {
835     delay(1);
836     counter++;
837   }
838   if (counter == 20)
839   {
840     pinMode(US_ECHO, OUTPUT);
841     digitalWrite(US_ECHO, HIGH);
842     delayMicroseconds(10);
843     digitalWrite(US_ECHO, LOW);
844     pinMode(US_ECHO, INPUT); 
845     delayMicroseconds(5);
846     distance = 10000;
847   }
848 }
849 
850 int16_t measure_distance()
851 {
852   start_distance_measurement();
853   wait_for_distance_measurement_to_complete();
854   return distance;
855 }

V programe puTTy môžete použiť aj tento jednoduchý robotí tanec

@ 1000 1 60 
1 5 90
1 6 90
1000 1 90
1 5 120 
1000 1 120
1 6 120
1 6 90 
1 2 120
1 5 90 
100  1 90
1 5 45  
100  2 90
1 6 45 
1000 2 60
1 5 120
1000 2 90
1 6 120 
1000 2 120
1 1 120
100 2 90
1 6 90
100 1 90
1 5 90 
0 0 0

ak toto vložíte do textove dokumentu a zkopírujete, pravým kliknitímna plochu v puTTy a následným stlačením tlačidla T váš robot začne tancovať