Difference between revisions of "Otto - riadiaci program v.2"
(8 intermediate revisions by 3 users not shown) | |||
Line 5: | Line 5: | ||
{|class="wikitable" | {|class="wikitable" | ||
− | !klaves!!servo!!pin!! | + | !klaves!!servo!!pin!!číslo serva v choreografii |
|- | |- | ||
− | |a/q || ľavá ruka || 10 || | + | |a/q || ľavá ruka || 10 ||5 |
|- | |- | ||
− | |;/p || pravá ruka|| 11 || | + | |;/p || pravá ruka|| 11 ||6 |
|- | |- | ||
− | |z/x || ľavá noha || 9 || | + | |z/x || ľavá noha || 9 ||4 |
|- | |- | ||
− | |,/. || pravá noha || 6 || | + | |,/. || pravá noha || 6 ||3 |
|- | |- | ||
− | |d/c || ľavá päta || 5 || | + | |d/c || ľavá päta || 5 ||2 |
|- | |- | ||
− | |k/m || pravá päta || 3 || | + | |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 - | + | ** 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''' | *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 243: | 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 421: | Line 461: | ||
int z; | int z; | ||
do { | do { | ||
− | z = | + | z = read_char(); |
− | if (z == '#') while (z != 13) z = | + | 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 428: | Line 468: | ||
num *= 10; | num *= 10; | ||
num += (z - '0'); | num += (z - '0'); | ||
− | do { z = | + | do { z = read_char(); if (z == -1) delayMicroseconds(10); } while (z < 0); |
} | } | ||
return num; | return num; | ||
Line 461: | 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 - 1; i++) | for (int i = 0; i < ch_len - 1; i++) | ||
Line 470: | 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 478: | 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 861: | Line 904: | ||
wait_for_distance_measurement_to_complete(); | wait_for_distance_measurement_to_complete(); | ||
return distance; | return distance; | ||
− | } | + | }</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ť