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ť riadky pc.begin(...);
a SoftwareSerial pc(...)
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[] = { 90, 90, 90, 90, 90, 90 };
47
48 Servo s[6];
49 SoftwareSerial pc(BT_TX, BT_RX);
50
51 uint16_t ch_time[CHOREO_LEN];
52 uint8_t ch_servo[CHOREO_LEN];
53 uint8_t ch_val[CHOREO_LEN];
54 int ch_len;
55 uint8_t kalib[6];
56 int stav[6];
57 int krok;
58
59 void setup() {
60 randomSeed(analogRead(1));
61 s[0].attach(SERVO_PRAVA_PATA);
62 s[1].attach(SERVO_LAVA_PATA);
63 s[2].attach(SERVO_PRAVA_NOHA);
64 s[3].attach(SERVO_LAVA_NOHA);
65 s[4].attach(SERVO_PRAVA_RUKA);
66 s[5].attach(SERVO_LAVA_RUKA);
67 for (int i = 0; i < 6; i++)
68 {
69 kalib[i] = prednastavena_kalibracia[i];
70 stav[i] = kalib[i];
71 s[i].write(stav[i]);
72 }
73 pc.begin(9600);
74 Serial.begin(9600);
75 ch_len = 0;
76 krok = 1;
77 ahoj();
78 ruky();
79 }
80
81 void pipni()
82 {
83 tone(SIRENA, 1568, 50);
84 delay(100);
85 tone(SIRENA, 1357, 50);
86 }
87
88 void ruky()
89 {
90 int odloz_krok = krok;
91 delay(500);
92 krok = 90;
93 pohyb(SERVO_LAVA_RUKA);
94 pohyb(SERVO_PRAVA_RUKA);
95 delay(1000);
96 krok = 180;
97 pohyb(-SERVO_LAVA_RUKA);
98 pohyb(-SERVO_PRAVA_RUKA);
99 delay(1000);
100 krok = odloz_krok;
101 pipni();
102 }
103
104 void ahoj()
105 {
106 tone(SIRENA, 1568, 50);
107 delay(70);
108 tone(SIRENA, 1175, 30);
109 delay(50);
110 tone(SIRENA, 880, 30);
111 delay(50);
112 tone(SIRENA, 1047, 50);
113 delay(70);
114 tone(SIRENA, 1245, 30);
115 delay(150);
116 tone(SIRENA, 1568, 50);
117 delay(100);
118 if (random(10) > 4) tone(SIRENA, 1357, 50);
119 else tone(SIRENA, 1047, 50);
120 }
121
122 void pohyb(int8_t servo)
123 {
124 int8_t srv = (servo > 0)?servo:-servo;
125 srv--;
126 if (servo_invertovane[srv]) servo = -servo;
127 if (servo > 0)
128 {
129 if (stav[srv] <= 180 - krok) stav[srv] += krok;
130 else stav[srv] = 180;
131 s[srv].write(stav[srv]);
132 }
133 else if (servo < 0)
134 {
135 if (stav[srv] >= krok) stav[srv] -= krok;
136 else stav[srv] = 0;
137 s[srv].write(stav[srv]);
138 }
139 }
140
141 uint8_t pohyb_znakom(char z)
142 {
143 for (int i = 0; i < 12; i++)
144 {
145 if (z == znaky_zmien[i])
146 {
147 int8_t servo = zmeny[i];
148 pohyb(servo);
149 }
150 }
151 }
152
153 void kombinacia1()
154 {
155 pohyb(SERVO_LAVA_NOHA);
156 pohyb(-SERVO_PRAVA_PATA);
157 }
158
159 void kombinacia2()
160 {
161 pohyb(SERVO_PRAVA_NOHA);
162 pohyb(-SERVO_LAVA_PATA);
163 }
164
165 void kombinacia3()
166 {
167 pohyb(SERVO_LAVA_RUKA);
168 pohyb(SERVO_PRAVA_RUKA);
169 }
170
171 void kombinacia4()
172 {
173 pohyb(-SERVO_LAVA_RUKA);
174 pohyb(-SERVO_PRAVA_RUKA);
175 }
176
177 int pohyb_kombinacia(char z)
178 {
179 if (z == '3') kombinacia1();
180 else if (z == '4') kombinacia2();
181 else if (z == '5') kombinacia3();
182 else if (z == '6') kombinacia4();
183 else return 0;
184 return 1;
185 }
186
187 int nacitajCislo()
188 {
189 int num = 0;
190 int z;
191 do {
192 z = pc.read();
193 if (z == '#') while (z != 13) z = pc.read();
194 } while ((z < '0') || (z > '9'));
195 while ((z >= '0') && (z <= '9'))
196 {
197 num *= 10;
198 num += (z - '0');
199 do { z = pc.read(); if (z == -1) delayMicroseconds(10); } while (z < 0);
200 }
201 return num;
202 }
203
204 void nacitaj_choreografiu()
205 {
206 ch_len = 0;
207 int tm;
208 do {
209 tm = nacitajCislo();
210 ch_time[ch_len] = tm;
211 ch_servo[ch_len] = nacitajCislo();
212 ch_val[ch_len] = nacitajCislo();
213 ch_len++;
214 if (ch_len == CHOREO_LEN) break;
215 } while (tm > 0);
216 pipni();
217 }
218
219 void vypis_choreografiu()
220 {
221 for (int i = 0; i < ch_len; i++)
222 {
223 pc.print(ch_time[i]);
224 pc.print(" ");
225 pc.print(ch_servo[i]);
226 pc.print(" ");
227 pc.println(ch_val[i]);
228 }
229 pipni();
230 }
231
232 void zatancuj_choreografiu()
233 {
234 for (int i = 0; i < ch_len; i++)
235 {
236 delay(ch_time[i]);
237 s[ch_servo[i] - 1].write(ch_val[i]);
238 }
239 pipni();
240 }
241
242 void reset()
243 {
244 for (int i = 0; i < 6; i++)
245 {
246 stav[i] = kalib[i];
247 s[i].write(kalib[i]);
248 }
249 pipni();
250 }
251
252 void kalibruj()
253 {
254 for (int i = 0; i < 6; i++)
255 {
256 pc.print(i);
257 pc.print(" (+/-/ENTER): ");
258 pc.println(kalib[i]);
259 s[i].write(kalib[i]);
260 char z;
261 do {
262 z = pc.read();
263 if ((z == '+') && (kalib[i] < 180)) { kalib[i]++; pc.print(kalib[i]); pc.print('\r'); s[i].write(kalib[i]); }
264 else if ((z == '-') && (kalib[i] > 0)) { kalib[i]--; pc.print(kalib[i]); pc.print('\r'); s[i].write(kalib[i]); }
265 } while (z != 13);
266 pc.print(i);
267 pc.print(": ");
268 pc.println(kalib[i]);
269 }
270 for (int i = 0; i < 6; i++) { pc.print(kalib[i]); pc.print(" "); }
271 pc.println("ok");
272 pipni();
273 }
274
275 void nacitaj_kalibraciu()
276 {
277 ch_len = 0;
278 int tm;
279 for (int i = 0; i < 6; i++)
280 kalib[i] = nacitajCislo();
281 for (int i = 0; i < 6; i++) { pc.print(kalib[i]); pc.print(" "); }
282 pc.println("ok");
283 pipni();
284 }
285
286 void zvys_krok()
287 {
288 if (krok < 180) krok++;
289 pc.print("krok: ");
290 pc.println(krok);
291 }
292
293 void zniz_krok()
294 {
295 if (krok > 0) krok--;
296 pc.print("krok: ");
297 pc.println(krok);
298 }
299
300 void loop() {
301 if (pc.available())
302 {
303 char z = pc.read();
304 if (pohyb_znakom(z)) return;
305 else if (pohyb_kombinacia(z)) return;
306 else if (z == '@') nacitaj_choreografiu();
307 else if (z == '?') vypis_choreografiu();
308 else if (z == 't') zatancuj_choreografiu();
309 else if (z == '-') ahoj();
310 else if (z == ' ') reset();
311 else if (z == 'H') kalibruj();
312 else if (z == 'L') nacitaj_kalibraciu();
313 else if (z == '9') zvys_krok();
314 else if (z == '1') zniz_krok();
315 }
316 }