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