Otto - riadiaci program v.1
Program umožňuje priame riadenie robota cez BlueTooth:
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 |
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),
- pozdraví po klávese -,
- 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ť riadok pc.begin(...);
1 #include <SoftwareSerial.h>
2 #include <Servo.h>
3
4 #define US_TRIG 7
5 #define US_ECHO 8
6
7 #define BT_RX 2
8 #define BT_TX 4
9
10 #define SIRENA 12
11
12 //maximalna dlzka choreografie
13 #define CHOREO_LEN 200
14
15 // v subore s choreografiou su serva cislovane takto:
16 //
17 // 1 - SERVO_PRAVA_PATA
18 // 2 - SERVO_LAVA_PATA
19 // 3 - SERVO_PRAVA_NOHA
20 // 4 - SERVO_LAVA_NOHA
21 // 5 - SERVO_PRAVA_RUKA
22 // 6 - SERVO_LAVA_RUKA
23
24 // cisla pinov, kde su zapojene servo motory
25 #define SERVO_LAVA_RUKA 10
26 #define SERVO_PRAVA_RUKA 11
27 #define SERVO_LAVA_NOHA 9
28 #define SERVO_PRAVA_NOHA 6
29 #define SERVO_LAVA_PATA 5
30 #define SERVO_PRAVA_PATA 3
31
32 // ak su niektore serva naopak, je tu jednotka
33 uint8_t servo_invertovane[6] = {0, 0, 1, 1, 0, 1};
34
35 // znaky, ktorymi sa ovladaju jednotlive stupne volnosti
36 char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' };
37 // co robia jednotlive znaky (znamienko urcuje smer)
38 int8_t zmeny[] = {-SERVO_LAVA_RUKA, SERVO_LAVA_RUKA,
39 -SERVO_PRAVA_RUKA, SERVO_PRAVA_RUKA,
40 -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA,
41 -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA,
42 SERVO_LAVA_PATA, -SERVO_LAVA_PATA,
43 SERVO_PRAVA_PATA, -SERVO_PRAVA_PATA };
44
45 // sem si mozno ulozit svoju kalibraciu
46 uint8_t prednastavena_kalibracia[] = { 78, 69, 83, 80, 50, 67 };
47 //uint8_t prednastavena_kalibracia = { 90, 90, 90, 90, 90, 90 };
48
49 Servo s[6];
50 SoftwareSerial pc(BT_TX, BT_RX);
51
52 uint16_t ch_time[CHOREO_LEN];
53 uint8_t ch_servo[CHOREO_LEN];
54 uint8_t ch_val[CHOREO_LEN];
55 int ch_len;
56 uint8_t kalib[6];
57 int stav[6];
58 int krok;
59
60 void setup() {
61 randomSeed(analogRead(1));
62 s[0].attach(SERVO_PRAVA_PATA);
63 s[1].attach(SERVO_LAVA_PATA);
64 s[2].attach(SERVO_PRAVA_NOHA);
65 s[3].attach(SERVO_LAVA_NOHA);
66 s[4].attach(SERVO_PRAVA_RUKA);
67 s[5].attach(SERVO_LAVA_RUKA);
68 for (int i = 0; i < 6; i++)
69 {
70 kalib[i] = prednastavena_kalibracia[i];
71 stav[i] = kalib[i];
72 s[i].write(stav[i]);
73 }
74 pc.begin(9600);
75 Serial.begin(9600);
76 ch_len = 0;
77 krok = 1;
78 ahoj();
79 ruky();
80 }
81
82 void pipni()
83 {
84 tone(SIRENA, 1568, 50);
85 delay(100);
86 tone(SIRENA, 1357, 50);
87 }
88
89 void ruky()
90 {
91 int odloz_krok = krok;
92 delay(500);
93 krok = 90;
94 pohyb(SERVO_LAVA_RUKA);
95 pohyb(SERVO_PRAVA_RUKA);
96 delay(1000);
97 krok = 180;
98 pohyb(-SERVO_LAVA_RUKA);
99 pohyb(-SERVO_PRAVA_RUKA);
100 delay(1000);
101 krok = odloz_krok;
102 pipni();
103 }
104
105 void ahoj()
106 {
107 tone(SIRENA, 1568, 50);
108 delay(70);
109 tone(SIRENA, 1175, 30);
110 delay(50);
111 tone(SIRENA, 880, 30);
112 delay(50);
113 tone(SIRENA, 1047, 50);
114 delay(70);
115 tone(SIRENA, 1245, 30);
116 delay(150);
117 tone(SIRENA, 1568, 50);
118 delay(100);
119 if (random(10) > 4) tone(SIRENA, 1357, 50);
120 else tone(SIRENA, 1047, 50);
121 }
122
123 void pohyb(int8_t servo)
124 {
125 int8_t srv = (servo > 0)?servo:-servo;
126 srv--;
127 if (servo_invertovane[srv]) servo = -servo;
128 if (servo > 0)
129 {
130 if (stav[srv] <= 180 - krok) stav[srv] += krok;
131 else stav[srv] = 180;
132 s[srv].write(stav[srv]);
133 }
134 else if (servo < 0)
135 {
136 if (stav[srv] >= krok) stav[srv] -= krok;
137 else stav[srv] = 0;
138 s[srv].write(stav[srv]);
139 }
140 }
141
142 uint8_t pohyb_znakom(char z)
143 {
144 for (int i = 0; i < 12; i++)
145 {
146 if (z == znaky_zmien[i])
147 {
148 int8_t servo = zmeny[i];
149 pohyb(servo);
150 }
151 }
152 }
153
154 void kombinacia1()
155 {
156 pohyb(SERVO_LAVA_NOHA);
157 pohyb(-SERVO_PRAVA_PATA);
158 }
159
160 void kombinacia2()
161 {
162 pohyb(SERVO_PRAVA_NOHA);
163 pohyb(-SERVO_LAVA_PATA);
164 }
165
166 void kombinacia3()
167 {
168 pohyb(SERVO_LAVA_RUKA);
169 pohyb(SERVO_PRAVA_RUKA);
170 }
171
172 void kombinacia4()
173 {
174 pohyb(-SERVO_LAVA_RUKA);
175 pohyb(-SERVO_PRAVA_RUKA);
176 }
177
178 int pohyb_kombinacia(char z)
179 {
180 if (z == '3') kombinacia1();
181 else if (z == '4') kombinacia2();
182 else if (z == '5') kombinacia3();
183 else if (z == '6') kombinacia4();
184 else return 0;
185 return 1;
186 }
187
188 int nacitajCislo()
189 {
190 int num = 0;
191 int z;
192 do {
193 z = pc.read();
194 if (z == '#') while (z != 13) z = pc.read();
195 } while ((z < '0') || (z > '9'));
196 while ((z >= '0') && (z <= '9'))
197 {
198 num *= 10;
199 num += (z - '0');
200 do { z = pc.read(); if (z == -1) delayMicroseconds(10); } while (z < 0);
201 }
202 return num;
203 }
204
205 void nacitaj_choreografiu()
206 {
207 ch_len = 0;
208 int tm;
209 do {
210 tm = nacitajCislo();
211 ch_time[ch_len] = tm;
212 ch_servo[ch_len] = nacitajCislo();
213 ch_val[ch_len] = nacitajCislo();
214 ch_len++;
215 if (ch_len == CHOREO_LEN) break;
216 } while (tm > 0);
217 pipni();
218 }
219
220 void vypis_choreografiu()
221 {
222 for (int i = 0; i < ch_len; i++)
223 {
224 pc.print(ch_time[i]);
225 pc.print(" ");
226 pc.print(ch_servo[i]);
227 pc.print(" ");
228 pc.println(ch_val[i]);
229 }
230 pipni();
231 }
232
233 void zatancuj_choreografiu()
234 {
235 for (int i = 0; i < ch_len; i++)
236 {
237 delay(ch_time[i]);
238 s[ch_servo[i] - 1].write(ch_val[i]);
239 }
240 pipni();
241 }
242
243 void reset()
244 {
245 for (int i = 0; i < 6; i++)
246 {
247 stav[i] = kalib[i];
248 s[i].write(kalib[i]);
249 }
250 pipni();
251 }
252
253 void kalibruj()
254 {
255 for (int i = 0; i < 6; i++)
256 {
257 pc.print(i);
258 pc.print(" (+/-/ENTER): ");
259 pc.println(kalib[i]);
260 s[i].write(kalib[i]);
261 char z;
262 do {
263 z = pc.read();
264 if ((z == '+') && (kalib[i] < 180)) { kalib[i]++; pc.print(kalib[i]); pc.print('\r'); s[i].write(kalib[i]); }
265 else if ((z == '-') && (kalib[i] > 0)) { kalib[i]--; pc.print(kalib[i]); pc.print('\r'); s[i].write(kalib[i]); }
266 } while (z != 13);
267 pc.print(i);
268 pc.print(": ");
269 pc.println(kalib[i]);
270 }
271 for (int i = 0; i < 6; i++) { pc.print(kalib[i]); pc.print(" "); }
272 pc.println("ok");
273 pipni();
274 }
275
276 void nacitaj_kalibraciu()
277 {
278 ch_len = 0;
279 int tm;
280 for (int i = 0; i < 6; i++)
281 kalib[i] = nacitajCislo();
282 for (int i = 0; i < 6; i++) { pc.print(kalib[i]); pc.print(" "); }
283 pc.println("ok");
284 pipni();
285 }
286
287 void zvys_krok()
288 {
289 if (krok < 180) krok++;
290 pc.print("krok: ");
291 pc.println(krok);
292 }
293
294 void zniz_krok()
295 {
296 if (krok > 0) krok--;
297 pc.print("krok: ");
298 pc.println(krok);
299 }
300
301 void loop() {
302 if (pc.available())
303 {
304 char z = pc.read();
305 if (pohyb_znakom(z)) return;
306 else if (pohyb_kombinacia(z)) return;
307 else if (z == '@') nacitaj_choreografiu();
308 else if (z == '?') vypis_choreografiu();
309 else if (z == 't') zatancuj_choreografiu();
310 else if (z == '-') ahoj();
311 else if (z == ' ') reset();
312 else if (z == 'H') kalibruj();
313 else if (z == 'L') nacitaj_kalibraciu();
314 else if (z == '9') zvys_krok();
315 else if (z == '1') zniz_krok();
316 }
317 }