Otto - riadiaci program v.1
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.
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), zobrazenie zadanej kalibrácie: klávesu G
- 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 Servo s[6];
63 //SoftwareSerial pc(BT_TX, BT_RX);
64
65 uint16_t ch_time[CHOREO_LEN];
66 uint8_t ch_servo[CHOREO_LEN];
67 uint8_t ch_val[CHOREO_LEN];
68 int ch_len;
69 uint8_t kalib[6];
70 int stav[6];
71 int krok;
72
73 void setup() {
74 randomSeed(analogRead(1));
75 s[0].attach(PIN_SERVO_PRAVA_PATA);
76 s[1].attach(PIN_SERVO_LAVA_PATA);
77 s[2].attach(PIN_SERVO_PRAVA_NOHA);
78 s[3].attach(PIN_SERVO_LAVA_NOHA);
79 s[4].attach(PIN_SERVO_PRAVA_RUKA);
80 s[5].attach(PIN_SERVO_LAVA_RUKA);
81 precitaj_kalibraciu_z_EEPROM();
82 for (int i = 0; i < 6; i++)
83 {
84 kalib[i] = prednastavena_kalibracia[i];
85 stav[i] = kalib[i];
86 s[i].write(stav[i]);
87 }
88 // pc.begin(9600);
89 Serial.begin(9600);
90 ch_len = 0;
91 krok = 7;
92 ahoj();
93 ruky();
94 }
95
96 void precitaj_kalibraciu_z_EEPROM()
97 {
98 uint8_t value = EEPROM.read(1);
99 if (value != '~') return;
100 for (int i = 2; i < 8; i++)
101 prednastavena_kalibracia[i - 2] = EEPROM.read(i);
102 }
103
104 void zapis_kalibraciu_do_EEPROM()
105 {
106 pc.print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
107 while (!pc.available());
108 char odpoved = pc.read();
109 pc.println(odpoved);
110 if (odpoved == 'Y')
111 {
112 EEPROM.write(1, '~');
113 for (int i = 2; i < 8; i++)
114 EEPROM.write(i, kalib[i - 2]);
115 pc.println("ok");
116 }
117 }
118
119 void pipni()
120 {
121 tone(SIRENA, 1568, 50);
122 delay(100);
123 tone(SIRENA, 1357, 50);
124 }
125
126 void ruky()
127 {
128 int odloz_krok = krok;
129 delay(500);
130 krok = 90;
131 pohyb(SERVO_LAVA_RUKA);
132 pohyb(SERVO_PRAVA_RUKA);
133 delay(1000);
134 krok = 180;
135 pohyb(-SERVO_LAVA_RUKA);
136 pohyb(-SERVO_PRAVA_RUKA);
137 delay(1000);
138 krok = odloz_krok;
139 pipni();
140 }
141
142 void ahoj()
143 {
144 tone(SIRENA, 1568, 50);
145 delay(70);
146 tone(SIRENA, 1175, 30);
147 delay(50);
148 tone(SIRENA, 880, 30);
149 delay(50);
150 tone(SIRENA, 1047, 50);
151 delay(70);
152 tone(SIRENA, 1245, 30);
153 delay(150);
154 tone(SIRENA, 1568, 50);
155 delay(100);
156 if (random(10) > 4) tone(SIRENA, 1357, 50);
157 else tone(SIRENA, 1047, 50);
158 }
159
160 void nastav_koncatinu(int8_t servo, uint8_t poloha)
161 {
162 int8_t srv = (servo > 0)?servo:-servo;
163 srv--;
164 poloha += prednastavena_kalibracia[srv] - 90;
165 if (poloha > 180) poloha = 180;
166 if (poloha < 0) poloha = 0;
167 stav[srv] = poloha;
168 s[srv].write(stav[srv]);
169 }
170
171 void pohyb(int8_t servo)
172 {
173 int8_t srv = (servo > 0)?servo:-servo;
174 srv--;
175 if (servo_invertovane[srv]) servo = -servo;
176 if (servo > 0)
177 {
178 if (stav[srv] <= 180 - krok) stav[srv] += krok;
179 else stav[srv] = 180;
180 s[srv].write(stav[srv]);
181 }
182 else if (servo < 0)
183 {
184 if (stav[srv] >= krok) stav[srv] -= krok;
185 else stav[srv] = 0;
186 s[srv].write(stav[srv]);
187 }
188 }
189
190 uint8_t pohyb_znakom(char z)
191 {
192 for (int i = 0; i < 12; i++)
193 {
194 if (z == znaky_zmien[i])
195 {
196 int8_t servo = zmeny[i];
197 pohyb(servo);
198 }
199 }
200 }
201
202 void kombinacia1()
203 {
204 pohyb(SERVO_LAVA_NOHA);
205 pohyb(-SERVO_PRAVA_PATA);
206 }
207
208 void kombinacia2()
209 {
210 pohyb(SERVO_PRAVA_NOHA);
211 pohyb(-SERVO_LAVA_PATA);
212 }
213
214 void kombinacia3()
215 {
216 pohyb(SERVO_LAVA_RUKA);
217 pohyb(SERVO_PRAVA_RUKA);
218 }
219
220 void kombinacia4()
221 {
222 pohyb(-SERVO_LAVA_RUKA);
223 pohyb(-SERVO_PRAVA_RUKA);
224 }
225
226 int pohyb_kombinacia(char z)
227 {
228 if (z == '3') kombinacia1();
229 else if (z == '4') kombinacia2();
230 else if (z == '5') kombinacia3();
231 else if (z == '6') kombinacia4();
232 else return 0;
233 return 1;
234 }
235
236 int nacitajCislo()
237 {
238 int num = 0;
239 int z;
240 do {
241 z = pc.read();
242 if (z == '#') while (z != 13) z = pc.read();
243 } while ((z < '0') || (z > '9'));
244 while ((z >= '0') && (z <= '9'))
245 {
246 num *= 10;
247 num += (z - '0');
248 do { z = pc.read(); if (z == -1) delayMicroseconds(10); } while (z < 0);
249 }
250 return num;
251 }
252
253 void nacitaj_choreografiu()
254 {
255 ch_len = 0;
256 int tm;
257 do {
258 tm = nacitajCislo();
259 ch_time[ch_len] = tm;
260 ch_servo[ch_len] = nacitajCislo();
261 ch_val[ch_len] = nacitajCislo();
262 ch_len++;
263 if (ch_len == CHOREO_LEN) break;
264 } while (tm > 0);
265 pipni();
266 }
267
268 void vypis_choreografiu()
269 {
270 for (int i = 0; i < ch_len; i++)
271 {
272 pc.print(ch_time[i]);
273 pc.print(" ");
274 pc.print(ch_servo[i]);
275 pc.print(" ");
276 pc.println(ch_val[i]);
277 }
278 pipni();
279 }
280
281 void zatancuj_choreografiu()
282 {
283 for (int i = 0; i < ch_len; i++)
284 {
285 delay(ch_time[i]);
286 nastav_koncatinu(ch_servo[i], ch_val[i]);
287 }
288 if (ch_len > 0) stav[ch_servo[ch_len - 1]] = ch_val[ch_len - 1];
289 pipni();
290 }
291
292 void reset()
293 {
294 for (int i = 0; i < 6; i++)
295 {
296 stav[i] = kalib[i];
297 s[i].write(kalib[i]);
298 }
299 pipni();
300 }
301
302 void kalibruj()
303 {
304 for (int i = 0; i < 6; i++)
305 {
306 pc.print(i);
307 pc.print(" (+/-/ENTER): ");
308 pc.println(kalib[i]);
309 s[i].write(kalib[i]);
310 char z;
311 do {
312 z = pc.read();
313 if ((z == '+') && (kalib[i] < 180)) { kalib[i]++; pc.print(kalib[i]); pc.print('\r'); s[i].write(kalib[i]); }
314 else if ((z == '-') && (kalib[i] > 0)) { kalib[i]--; pc.print(kalib[i]); pc.print('\r'); s[i].write(kalib[i]); }
315 } while (z != 13);
316 pc.print(i);
317 pc.print(": ");
318 pc.println(kalib[i]);
319 }
320 for (int i = 0; i < 6; i++) { pc.print(kalib[i]); pc.print(" "); }
321 pc.println("ok");
322 pipni();
323 }
324
325
326 void vypis_kalibraciu()
327 {
328 for (int i = 0; i < 6; i++) { pc.print(kalib[i]); pc.print(" "); }
329 pc.println();
330 }
331
332 void nacitaj_kalibraciu()
333 {
334 int tm;
335 for (int i = 0; i < 6; i++)
336 kalib[i] = nacitajCislo();
337 vypis_kalibraciu();
338 pc.println("ok");
339 pipni();
340 }
341
342 void zvys_krok()
343 {
344 if (krok < 180) krok++;
345 pc.print("krok: ");
346 pc.println(krok);
347 }
348
349 void zniz_krok()
350 {
351 if (krok > 0) krok--;
352 pc.print("krok: ");
353 pc.println(krok);
354 }
355
356 void loop() {
357 if (pc.available())
358 {
359 char z = pc.read();
360 if (pohyb_znakom(z)) return;
361 else if (pohyb_kombinacia(z)) return;
362 else if (z == '@') nacitaj_choreografiu();
363 else if (z == '?') vypis_choreografiu();
364 else if (z == 't') zatancuj_choreografiu();
365 else if (z == '-') ahoj();
366 else if (z == ' ') reset();
367 else if (z == 'H') kalibruj();
368 else if (z == 'G') vypis_kalibraciu();
369 else if (z == 'L') nacitaj_kalibraciu();
370 else if (z == 'E') zapis_kalibraciu_do_EEPROM();
371 else if (z == 'R') ruky();
372 else if (z == '9') zvys_krok();
373 else if (z == '1') zniz_krok();
374 }
375 }