Difference between revisions of "Otto - riadiaci program v.1"
m |
|||
(3 intermediate revisions by 2 users not shown) | |||
Line 1: | Line 1: | ||
Program umožňuje priame riadenie robota (aj 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. | 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 32: | 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 | ||
+ | ** 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''' | *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!), po zapnutí sa automaticky načíta | ||
Line 220: | Line 226: | ||
int8_t srv = (servo > 0)?servo:-servo; | int8_t srv = (servo > 0)?servo:-servo; | ||
srv--; | srv--; | ||
− | poloha += | + | poloha += kalib[srv] - 90; |
if (poloha > 180) poloha = 180; | if (poloha > 180) poloha = 180; | ||
if (poloha < 0) poloha = 0; | if (poloha < 0) poloha = 0; |
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 }