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

From DT^2
Jump to: navigation, search
m
Line 1: Line 1:
 +
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: [https://the.earth.li/~sgtatham/putty/latest/w64/putty.exe putty.exe].
 +
 +
{|class="wikitable"
 +
!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
 +
*Kláves '''S''' zobrazí aktuálnu polohu servomotorov
 +
*'''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>
 
#include <Servo.h>
 
#include <Servo.h>
Line 583: Line 634:
 
     nastav_koncatinu(ch_servo[i], 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();
 
   while (serial_available()) serial_read();
 
   while (serial_available()) serial_read();

Revision as of 05:49, 9 August 2018

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

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

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
  • Kláves S zobrazí aktuálnu polohu servomotorov
  • 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 uint8_t spomalenie;
 74 
 75 static volatile int16_t distance;
 76 
 77 uint8_t *melodia;
 78 uint16_t dlzka_melodie;
 79 
 80 #define FIS3 2960
 81 #define G3 3136
 82 
 83 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 };
 84 int16_t music_speed;
 85 
 86 uint16_t dlzka_popcorn = 390;
 87 uint8_t popcorn[] = { 149, 49, 
 88 28, 31, 35, 40, 49, 99, 38, 49, 99, 40, 49, 99, 35, 49, 99, 31, 49, 99, 35, 49, 99, 28, 49, 99, 49,
 89 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,
 90 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, 
 91 28, 31, 35, 40, 49, 99, 38, 49, 99, 40, 49, 99, 35, 49, 99, 31, 49, 99, 35, 49, 99, 28, 49, 99, 49,
 92 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,
 93 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,
 94 49, 35, 38, 43, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 38, 49, 99, 43, 49, 99, 35, 49, 99, 
 95 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 ,
 96 40, 49, 99, 47, 49, 99, 254, 49, 99, 255, 49, 99, 254, 49, 99, 255, 49, 99, 47, 49, 99, 254, 49, 99, 47, 49, 99, 254, 49, 99, 45, 49, 99, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 47, 49, 99, 
 97 49, 35, 38, 43, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 38, 49, 99, 43, 49, 99, 35, 49, 99, 
 98 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 ,
 99 40, 49, 99, 47, 49, 99, 254, 49, 99, 255, 49, 99, 254, 49, 99, 255, 49, 99, 47, 49, 99, 254, 49, 99, 47, 49, 99, 254, 49, 99, 45, 49, 99, 47, 49, 99, 45, 49, 99, 47, 49, 99, 254, 49, 99, 255, 49, 99
100 };
101 
102 void setup() {
103   Serial.begin(9600);
104   init_serial(9600);
105   init_ultrasonic();
106 
107   melodia = popcorn;
108   dlzka_melodie = dlzka_popcorn;
109 
110   randomSeed(analogRead(1));
111   s[0].attach(PIN_SERVO_PRAVA_PATA);
112   s[1].attach(PIN_SERVO_LAVA_PATA);
113   s[2].attach(PIN_SERVO_PRAVA_NOHA);
114   s[3].attach(PIN_SERVO_LAVA_NOHA);
115   s[4].attach(PIN_SERVO_LAVA_RUKA);
116   s[5].attach(PIN_SERVO_PRAVA_RUKA);
117   precitaj_kalibraciu_z_EEPROM();
118   for (int i = 0; i < 6; i++)
119   {
120     kalib[i] = prednastavena_kalibracia[i];
121     stav[i] = kalib[i];
122     s[i].write(stav[i]);
123   }
124   ch_len = 0;
125   krok = 7;
126   spomalenie = 6;
127   ahoj();
128   ruky2();
129   delay(100);  
130   serial_println("\r\n  Otto DTDT");
131 }
132 
133 void loop() {
134   char z = -1;
135   if (serial_available()) z = serial_read();
136 #ifdef ECHO_BT_TO_USB
137     if (Serial.available()) z = Serial.read();
138 #endif  
139   
140   if (z != -1)
141   { 
142     if (pohyb_znakom(z)) return;   
143     else if (pohyb_kombinacia(z)) return; 
144     else if (z == '@') nacitaj_choreografiu();
145     else if (z == '?') vypis_choreografiu();
146     else if (z == 't') zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
147     else if (z == '-') ahoj();
148     else if (z == ' ') reset();
149     else if (z == 'H') kalibruj();
150     else if (z == 'J') nastav_limity();
151     else if (z == 'G') vypis_kalibraciu();
152     else if (z == 'L') nacitaj_kalibraciu();
153     else if (z == 'E') zapis_kalibraciu_do_EEPROM();
154     else if (z == 'R') ruky();
155     else if (z == '9') zvys_krok();
156     else if (z == '1') zniz_krok();
157     else if (z == '8') zvys_spomalenie();
158     else if (z == '7') zniz_spomalenie();
159     else if (z == 'U') test_ultrazvuk();
160     else if (z == 'S') zobraz_stav();
161   }
162   int16_t d = measure_distance();
163   if (d < 10) menu_ultrasonic_request();
164 }
165 
166 void otto_translate_tone(uint8_t *note, uint16_t *freq, uint16_t *del)
167 {
168   if (*note == 251)
169   {
170     *freq = ((*(note + 1)) << 8) + (*(note + 2));
171     uint8_t d1 = *(note + 3);
172     uint8_t d2 = *(note + 4);
173     *del = (music_speed * 16 * (long)d1) / d2;
174   }
175   else if (*note == 254)
176   {
177     *freq = FIS3;
178 	*del = music_speed;
179   }
180   else if (*note == 255)
181   {
182     *freq = G3;
183 	*del = music_speed;
184   }
185   else
186   {
187     uint8_t len = *note / 50; 
188     *del = music_speed;
189     while (len--) *del *= 2;
190     uint8_t n = *note % 50;
191     if (n != 49)
192     {
193       uint8_t octave = (n + 5) / 12;
194       n = (n + 5 ) % 12;
195       float ffreq = octave_4[n];
196       octave = 4 - octave;
197       while (octave > 0)
198       {
199         ffreq /= 2.0;
200         octave--;  
201       }
202 	  *freq = (uint16_t)ffreq;
203     }
204 	else *ffreq = 0;
205   }
206 }
207 
208 void otto_tone(uint8_t *note)
209 {
210   uint16_t freq, d;
211   otto_translate_tone(note, &freq, &d);
212   if (freq) tone(SIRENA, freq); else noTone(SIRENA);
213   delay(d);
214   noTone(SIRENA);
215 }
216 
217 void zahraj_melodiu() 
218 {
219   music_speed = 800 / 16;
220   for (int j = 0; j < 4; j++)
221   {
222     for (int i = 0; i < dlzka_melodie; i++)
223     {
224       otto_tone(melodia + i);
225       if (melodia[i] == 251) i += 4;
226     }
227     music_speed -= 150 / 16;
228   }
229   delay(3000);
230 }
231 
232 void test_ultrazvuk()
233 {
234   int i = 0;
235   while ((serial_available() == 0) && (Serial.available() == 0))
236   {
237     serial_println_num(measure_distance());
238     delay(100);
239   }
240   serial_read();
241 }
242 
243 void menu_ultrasonic_request()
244 {
245   uint32_t tm = millis();
246   int d = measure_distance();
247   int count = 0;
248   int count2 = 0;
249   while ((millis() - tm < 1500) && ((d < 15) || (count2 < 5)) && (count < 10)) 
250   {
251     delay(10);
252     d = measure_distance();
253     if (d == 10000) count++;
254     else count = 0;
255     if (d >= 15) count2++;
256     else count2 = 0;
257 	if (serial_available() || Serial.available()) return;
258   }
259   if (millis() - tm >= 1500)
260     ultrasonic_menu();
261 }
262 
263 void ultrasonic_menu()
264 {
265   int selection = 0;
266   tone(SIRENA, 880, 200);
267   
268   do {  
269     int count = 0;
270     do { 
271       int32_t d = measure_distance();
272       if (d == 10000) continue;
273       if (d >= 20) count++;
274       else count = 0;
275       delay(10);
276     } while (!serial_available() && !Serial.available() && (count < 20));
277     
278     tone(SIRENA, 440, 200);
279     uint32_t tm = millis();
280     while ((measure_distance() > 15) && (millis() - tm < 1500) && !serial_available() && !Serial.available()) delay(10);
281     if (millis() - tm >= 1500) 
282     {
283       tone(SIRENA, 2000, 50);
284       menu_command(selection);
285       return;
286     }       
287     selection++;
288     for (int i = 0; i < selection; i++)
289     {
290       tone(SIRENA, 1261, 50);
291       delay(250);
292     }
293   } while (!serial_available() && !Serial.available());
294   while (serial_available()) serial_read();
295   while (Serial.available()) Serial.read();
296 }
297 
298 void menu_command(int cmd)
299 {
300   if (cmd == 1) vpred();
301   if (cmd == 2) zahraj_melodiu();
302   if (cmd == 3) melodia_jedna_druhej();
303   if (cmd == 4) ahoj();
304   serial_println_num(cmd);
305 }
306 
307 void melodia_jedna_druhej()
308 {
309   for (int i = 0; i < 2; i++)
310   {
311     tone(SIRENA, 262, 200);
312     delay(200);
313     tone(SIRENA, 330, 200);
314     delay(200);
315   
316     tone(SIRENA, 262, 200);
317     delay(200);
318     tone(SIRENA, 330, 200);
319     delay(200);
320   
321     tone(SIRENA, 392, 400);
322     delay(400);
323   
324     tone(SIRENA, 392, 400);
325     delay(400);
326   }
327   noTone(8);
328 }
329 
330 // chodza pre vacsieho dreveneho robota
331 uint16_t chor_time[] = {100,1,1,100,1,1,1,100,1,1,1,100,1,100,1,0};
332 uint8_t chor_servo[] = {1,2,6,4,3,6,5,1,2,6,5,3,4,1,2,0};
333 uint8_t chor_val[] = {48,69,180,104,104,90,0,111,146,0,90,62,69,48,69,0};
334 uint8_t chor_len = 16;
335 
336 void vpred()
337 {
338   while (measure_distance() > 30)    
339     zatancuj_choreografiu(chor_time, chor_servo, chor_val, chor_len);
340   pipni();
341   reset();
342 }
343 
344 void precitaj_kalibraciu_z_EEPROM()
345 {
346   uint8_t value = EEPROM.read(1);
347   if (value != '~') return;
348   for (int i = 2; i < 8; i++)
349     prednastavena_kalibracia[i - 2] = EEPROM.read(i);
350   for (int i = 0; i < 6; i++)
351     dolny_limit[i] = EEPROM.read(i + 9);
352   for (int i = 0; i < 6; i++)
353     horny_limit[i] = EEPROM.read(i + 15);    
354 }
355 
356 char read_char()
357 {
358   while (!serial_available() && !Serial.available());
359   if (serial_available()) return serial_read();
360   else return Serial.read();
361 }
362 
363 void zapis_kalibraciu_do_EEPROM()
364 {
365   serial_print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
366   char odpoved = read_char();
367   serial_println_char(odpoved);
368   if (odpoved == 'Y') 
369   {
370     EEPROM.write(1, '~');
371     for (int i = 2; i < 8; i++)
372       EEPROM.write(i, kalib[i - 2]);
373     for (int i = 0; i < 6; i++)
374       EEPROM.write(9 + i, dolny_limit[i]);
375     for (int i = 0; i < 6; i++)
376       EEPROM.write(15 + i, horny_limit[i]);
377     serial_println("ok");
378   }
379 }
380 
381 void pipni()
382 {
383   tone(SIRENA, 1568, 50);
384   delay(100);
385   tone(SIRENA, 1357, 50);
386 }
387 
388 void ruky()
389 {
390   int odloz_krok = krok;
391   delay(500);
392   krok = 90;
393   pohyb(SERVO_LAVA_RUKA);    
394   pohyb(SERVO_PRAVA_RUKA);
395   delay(1000);
396   krok = 180;
397   pohyb(-SERVO_LAVA_RUKA);    
398   pohyb(-SERVO_PRAVA_RUKA);
399   delay(1000);
400   krok = odloz_krok;
401   pipni();
402 }
403 
404 void ruky2()
405 {
406   int odloz_krok = krok;
407   delay(500);
408   krok = 180;
409   pohyb(SERVO_LAVA_RUKA);    
410   pohyb(SERVO_PRAVA_RUKA);
411   delay(1000);
412   krok = 90;
413   pohyb(-SERVO_LAVA_RUKA);    
414   pohyb(-SERVO_PRAVA_RUKA);
415   delay(1000);
416   krok = odloz_krok;
417   pipni();
418 }
419 
420 void ahoj()
421 {
422   tone(SIRENA, 1568, 50);
423   delay(70);
424   tone(SIRENA, 1175, 30);
425   delay(50);
426   tone(SIRENA, 880, 30);
427   delay(50);
428   tone(SIRENA, 1047, 50);
429   delay(70);
430   tone(SIRENA, 1245, 30);
431   delay(150);
432   tone(SIRENA, 1568, 50);
433   delay(100);
434   if (random(10) > 4) tone(SIRENA, 1357, 50);
435   else tone(SIRENA, 1047, 50);
436 }
437 
438 void nastav_koncatinu(int8_t servo, uint8_t poloha)
439 {
440   int8_t delta;
441   int8_t srv = (servo > 0)?servo:-servo;
442   srv--;
443   poloha += kalib[srv] - 90;
444   if (poloha > 180) poloha = 180;
445   if (poloha < 0) poloha = 0;
446 
447   if (stav[srv] < poloha) delta = 1;
448   else delta = -1;
449   while (stav[srv] != poloha)
450   {
451     stav[srv] +=delta;
452     s[srv].write(stav[srv]);
453     delay(spomalenie);
454   }
455 }
456 
457 void zobraz_stav()
458 {
459   for (int i = 0; i < 6; i++)
460   {
461     serial_print("S"); serial_print_num(i + 1); serial_print(": "); serial_println_num(stav[i] - kalib[i] + 90);
462   }
463   serial_println("---");
464   pipni();
465 }
466 
467 void pohyb(int8_t servo)
468 {
469   int8_t srv = (servo > 0)?servo:-servo;
470   srv--;
471   if (servo_invertovane[srv]) servo = -servo;
472   if (servo > 0)
473   {
474     if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
475     else stav[srv] = horny_limit[srv];
476     s[srv].write(stav[srv]);
477   }
478   else if (servo < 0)
479   {
480     if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok; 
481     else stav[srv] = dolny_limit[srv];
482     s[srv].write(stav[srv]);      
483   }
484 }
485 
486 uint8_t pohyb_znakom(char z)
487 {
488   for (int i = 0; i < 12; i++)
489   {
490     if (z == znaky_zmien[i])
491     {
492       int8_t servo = zmeny[i];
493       pohyb(servo);
494     }
495   }
496 }
497 
498 void kombinacia1()
499 {
500   pohyb(SERVO_LAVA_NOHA);
501   pohyb(-SERVO_PRAVA_PATA);
502 }
503 
504 void kombinacia2()
505 {
506   pohyb(SERVO_PRAVA_NOHA);
507   pohyb(-SERVO_LAVA_PATA);
508 }
509 
510 void kombinacia3()
511 {
512    pohyb(SERVO_LAVA_RUKA); 
513    pohyb(SERVO_PRAVA_RUKA); 
514 }
515 
516 void kombinacia4()
517 {
518    pohyb(-SERVO_LAVA_RUKA); 
519    pohyb(-SERVO_PRAVA_RUKA); 
520 }
521 
522 int pohyb_kombinacia(char z)
523 {
524   if (z == '3') kombinacia1();
525   else if (z == '4') kombinacia2();
526   else if (z == '5') kombinacia3();
527   else if (z == '6') kombinacia4();
528   else return 0;
529   return 1;
530 }
531 
532 int nacitajCislo()
533 {
534   int num = 0;
535   int z;
536   do {
537     z = read_char();
538     if (z == '#') while (z != 13) z = read_char();
539   } while ((z < '0') || (z > '9'));
540   while ((z >= '0') && (z <= '9'))
541   {
542     num *= 10;
543     num += (z - '0');
544     do { z = read_char(); if (z == -1) delayMicroseconds(10); } while (z < 0);
545   }
546   return num;
547 }
548 
549 void nacitaj_choreografiu()
550 {
551   ch_len = 0;
552   int tm;
553   do { 
554     tm = nacitajCislo();
555     ch_time[ch_len] = tm;
556     ch_servo[ch_len] = nacitajCislo();
557     ch_val[ch_len] = nacitajCislo();
558     ch_len++;
559   if (ch_len == CHOREO_LEN) break;
560   } while (tm > 0);
561   pipni();  
562 }
563 
564 void vypis_choreografiu()
565 {
566   for (int i = 0; i < ch_len; i++)
567   {
568     serial_print_num(ch_time[i]);
569     serial_print(" ");
570     serial_print_num(ch_servo[i]);
571     serial_print(" ");
572     serial_println_num(ch_val[i]);      
573   }
574   pipni();
575 }
576 
577 void zatancuj_choreografiu(uint16_t *ch_time, uint8_t *ch_servo, uint8_t *ch_val, int ch_len )
578 {
579   for (int i = 0; i < ch_len - 1; i++)
580   {
581     delay(ch_time[i]);
582     nastav_koncatinu(ch_servo[i], ch_val[i]);
583   }
584   pipni();
585   while (serial_available()) serial_read();
586   while (Serial.available()) Serial.read();
587 }
588 
589 void reset()
590 {
591   for (int i = 0; i < 6; i++) 
592   {
593     stav[i] = kalib[i];
594     s[i].write(kalib[i]);
595     delay(300);
596   }
597   pipni();
598 }
599 
600 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
601 {
602     serial_print(" (+/-/ENTER): ");
603     serial_println_num(hodnota);
604     s[servo].write(hodnota);
605     char z;
606     do {
607       z = read_char();
608       if ((z == '+') && (hodnota < 180)) hodnota++;
609       else if ((z == '-') && (hodnota > 0)) hodnota--; 
610       if ((z == '+') || (z == '-'))
611       {
612         serial_print_num(hodnota); serial_print_char('\r'); 
613         s[servo].write(hodnota);
614       }
615     } while (z != 13);
616     return hodnota;
617 }
618 
619 void kalibruj()
620 {
621   for (int i = 0; i < 6; i++)
622   {
623     serial_print_num(i);
624     kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
625     serial_print_num(i);
626     serial_print(": ");
627     serial_println_num(kalib[i]);
628   }
629   for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
630   serial_println("ok");
631   pipni();
632 }
633 
634 void nastav_limity()
635 {
636   for (int i = 0; i < 6; i++)
637   {
638     serial_print_num(i);
639     serial_print("dolny");
640     dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
641     serial_print_num(i);
642     serial_print(" dolny: ");
643     serial_println_num(dolny_limit[i]);
644     s[i].write(kalib[i]);
645 
646     serial_print_num(i);
647     serial_print("horny");
648     horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
649     serial_print_num(i);
650     serial_print(" horny: ");
651     serial_println_num(horny_limit[i]);
652     s[i].write(kalib[i]);
653   }
654   for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print("-"); serial_print_num(horny_limit[i]); serial_print(" "); }
655   serial_println("ok");
656   pipni();
657 }
658 
659 void vypis_kalibraciu()
660 {
661   serial_print("stredy: ");
662   for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
663   serial_println();
664   serial_print("dolny limit: ");
665   for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print(" "); }
666   serial_println();
667   serial_print("horny limit: ");
668   for (int i = 0; i < 6; i++) { serial_print_num(horny_limit[i]); serial_print(" "); }
669   serial_println();
670 }
671 
672 void nacitaj_kalibraciu()
673 {
674   int tm;
675   for (int i = 0; i < 6; i++) 
676     kalib[i] = nacitajCislo();
677   vypis_kalibraciu();
678   serial_println("ok");
679   pipni();
680 }
681 
682 void zvys_krok()
683 {
684   if (krok < 180) krok++;
685   serial_print("krok: ");
686   serial_println_num(krok);
687 }
688 
689 void zniz_krok()
690 {
691   if (krok > 0) krok--;
692   serial_print("krok: ");
693   serial_println_num(krok);
694 }
695 
696 void zniz_spomalenie()
697 {
698   if (spomalenie > 0) spomalenie--;
699   serial_print("spomalenie: "); 
700   serial_println_num(spomalenie);
701 }
702 
703 void zvys_spomalenie()
704 {
705   if (spomalenie < 100) spomalenie++;
706   serial_print("spomalenie: ");
707   serial_println_num(spomalenie);
708 }
709 
710 // nasleduje softverova implementacia serioveho portu
711 #define SERIAL_STATE_IDLE      0
712 #define SERIAL_STATE_RECEIVING 1
713 #define SERIAL_BUFFER_LENGTH   20
714 
715 static volatile uint8_t serial_state;
716 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
717 static volatile uint8_t serial_buf_wp, serial_buf_rp;
718 
719 static volatile uint8_t receiving_byte;
720 
721 static volatile uint32_t time_startbit_noticed;
722 static volatile uint8_t next_bit_order;
723 static volatile uint8_t waiting_stop_bit;
724 static uint16_t one_byte_duration;
725 static uint16_t one_bit_duration;
726 static uint16_t one_bit_write_duration;
727 static uint16_t half_of_one_bit_duration;
728 
729 void init_serial(uint32_t baud_rate)
730 {
731   pinMode(2, INPUT);
732   pinMode(4, OUTPUT);
733   
734   serial_state = SERIAL_STATE_IDLE;
735   
736   one_byte_duration = 9500000 / baud_rate;
737   one_bit_duration = 1000000 / baud_rate;
738   one_bit_write_duration = one_bit_duration - 1;
739   half_of_one_bit_duration = 500000 / baud_rate;
740   
741   PCMSK2 |= 4; //PCINT18;
742   PCIFR &= ~4; //PCIF2;
743   PCICR |= 4; // PCIE2;
744 }
745 
746 ISR(PCINT2_vect)
747 {
748   uint32_t tm = micros();
749   if (serial_state == SERIAL_STATE_IDLE)
750   {    
751     time_startbit_noticed = tm;
752     serial_state = SERIAL_STATE_RECEIVING;
753     receiving_byte = 0xFF;
754     next_bit_order = 0;
755   }
756   else if (tm - time_startbit_noticed > one_byte_duration)
757   {
758       serial_buffer[serial_buf_wp] = receiving_byte;
759       serial_buf_wp++;
760       if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
761       time_startbit_noticed = tm;
762       receiving_byte = 0xFF;
763       next_bit_order = 0;
764   }
765   else if (PIND & 4) 
766   {
767      int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
768      while (next_bit_order < new_next_bit_order)
769      {  
770         receiving_byte &= ~(1 << next_bit_order);
771         next_bit_order++;
772      }
773      if (next_bit_order == 8)
774      { 
775         serial_buffer[serial_buf_wp] = receiving_byte;
776         serial_buf_wp++;
777         if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
778         serial_state = SERIAL_STATE_IDLE;
779      }        
780   } else 
781       next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
782 }
783 
784 uint8_t serial_available()
785 {
786   cli();
787   if (serial_buf_rp != serial_buf_wp) 
788   {
789     sei();
790     return 1;
791   }
792   if (serial_state == SERIAL_STATE_RECEIVING)
793   {
794     uint32_t tm = micros();
795     if (tm - time_startbit_noticed > one_byte_duration)
796     {      
797       serial_state = SERIAL_STATE_IDLE;
798       serial_buffer[serial_buf_wp] = receiving_byte;
799       serial_buf_wp++;
800       if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
801       sei();
802       return 1;
803     }
804   }
805   sei();
806   return 0;
807 }
808 
809 int16_t serial_read()
810 {
811   cli();
812   if (serial_buf_rp != serial_buf_wp)
813   {
814     uint8_t ch = serial_buffer[serial_buf_rp];
815     serial_buf_rp++;
816     if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
817     sei();
818     return ch;
819   }
820 
821   if (serial_state == SERIAL_STATE_RECEIVING)
822   {
823     uint32_t tm = micros();
824     if (tm - time_startbit_noticed > one_byte_duration)
825     {
826       uint8_t ch = receiving_byte;
827       serial_state = SERIAL_STATE_IDLE;  
828       sei();
829       return ch;
830     }
831   }
832   sei();
833   return -1;
834 }
835 
836 void serial_write(uint8_t ch)
837 {
838 #ifdef ECHO_BT_TO_USB
839   Serial.print((char)ch);
840 #endif
841   PORTD &= ~16;
842   delayMicroseconds(one_bit_write_duration);
843   for (uint8_t i = 0; i < 8; i++)
844   {
845     if (ch & 1) PORTD |= 16;
846     else PORTD &= ~16;
847     ch >>= 1;
848     delayMicroseconds(one_bit_write_duration);
849   }
850   PORTD |= 16;
851   delayMicroseconds(one_bit_write_duration);
852   delayMicroseconds(one_bit_write_duration);
853   delayMicroseconds(one_bit_write_duration);
854   delayMicroseconds(one_bit_write_duration);
855   delayMicroseconds(one_bit_write_duration);
856 }
857 
858 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
859 {
860   uint16_t len;
861   uint16_t ch;
862   do {
863     ch = serial_read();
864     if (ch == 13) continue;
865   } while (ch == -1);
866 
867   do {
868     if ((ch != 13) && (ch != 10) && (ch != -1)) 
869     {
870       *(ln++) = ch;
871       max_length--;
872       len++;
873     }    
874     ch = serial_read();
875   } while ((ch != 13) && max_length);
876   *ln = 0;
877   return len;
878 }
879 
880 void serial_print_num(int32_t number)
881 {
882   if (number < 0) 
883   {
884     serial_write('-');
885     number = -number;
886   }
887   int32_t rad = 1;
888   while (number / rad) rad *= 10;
889   if (number > 0) rad /= 10;
890   while (rad)
891   {
892     serial_write((char)('0' + (number / rad)));
893     number -= (number / rad) * rad;
894     rad /= 10;
895   }  
896 }
897 
898 void serial_print_char(char ch)
899 {
900   serial_write(ch);  
901 }
902 
903 void serial_print(const uint8_t *str)
904 {
905   while (*str) serial_write(*(str++));
906 }
907 
908 void serial_println(const uint8_t *str)
909 {
910   serial_print(str);
911   serial_write(13);
912   serial_write(10);
913 }
914 
915 void serial_println_num(int32_t number)
916 {
917   serial_print_num(number);
918   serial_println();
919 }
920 
921 void serial_println_char(char ch)
922 {
923   serial_write(ch);
924   serial_println();
925 }
926 
927 void serial_println()
928 {
929   serial_write(13);
930   serial_write(10);
931 }
932 
933 // nasleduje citanie z utltazvukoveho senzora
934 
935 static volatile uint32_t pulse_start;
936 static volatile uint8_t new_distance;
937 
938 void init_ultrasonic()
939 {
940   pinMode(US_ECHO, INPUT);
941   pinMode(US_TRIG, OUTPUT);
942   
943   PCMSK0 |= 1; //PCINT0;
944   PCIFR &= ~1; //PCIF0;
945   PCICR |= 1; // PCIE0;  
946 }
947 
948 ISR(PCINT0_vect)
949 {
950   if (PINB & 1) pulse_start = micros();
951   else 
952   {
953     distance = (int16_t)((micros() - pulse_start) / 58);
954     new_distance = 1;
955   }
956 }
957 
958 void start_distance_measurement()
959 {
960   distance = 10000;
961   new_distance = 0;
962   digitalWrite(US_TRIG, HIGH);
963   delayMicroseconds(10);
964   digitalWrite(US_TRIG, LOW);
965 }
966 
967 void wait_for_distance_measurement_to_complete()
968 {
969   uint8_t counter = 0;
970   while ((counter < 20) && !new_distance) 
971   {
972     delay(1);
973     counter++;
974   }
975   if (counter == 20)
976   {
977     pinMode(US_ECHO, OUTPUT);
978     digitalWrite(US_ECHO, HIGH);
979     delayMicroseconds(10);
980     digitalWrite(US_ECHO, LOW);
981     pinMode(US_ECHO, INPUT); 
982     delayMicroseconds(5);
983     distance = 10000;
984   }
985 }
986 
987 int16_t measure_distance()
988 {
989   start_distance_measurement();
990   wait_for_distance_measurement_to_complete();
991   return distance;
992 }