Difference between revisions of "Otto - riadiaci program v.3"
(Created page with "<syntaxhighlight lang="C" line=line> #include <Servo.h> #include <EEPROM.h> #define ECHO_BT_TO_USB 1 #define US_TRIG 7 #define US_ECHO 8 #define BT_RX 2 #define BT_TX...") |
m |
||
Line 78: | Line 78: | ||
uint8_t *melodia; | uint8_t *melodia; | ||
uint16_t dlzka_melodie; | uint16_t dlzka_melodie; | ||
− | |||
#define FIS3 2960 | #define FIS3 2960 | ||
Line 101: | Line 100: | ||
40, 49, 99, 47, 49, 99, 254, 49, 99, 255, 49, 99, 254, 49, 99, 255, 49, 99, 47, 49, 99, 254, 49, 99, 47, 49, 99, 254, 49, 99, 45, 49, 99, 47, 49, 99, 45, 49, 99, 47, 49, 99, 254, 49, 99, 255, 49, 99 | 40, 49, 99, 47, 49, 99, 254, 49, 99, 255, 49, 99, 254, 49, 99, 255, 49, 99, 47, 49, 99, 254, 49, 99, 47, 49, 99, 254, 49, 99, 45, 49, 99, 47, 49, 99, 45, 49, 99, 47, 49, 99, 254, 49, 99, 255, 49, 99 | ||
}; | }; | ||
− | |||
void setup() { | void setup() { | ||
Line 167: | Line 165: | ||
} | } | ||
− | + | void otto_translate_tone(uint8_t *note, uint16_t *freq, uint16_t *del) | |
− | |||
− | void | ||
{ | { | ||
if (*note == 251) | if (*note == 251) | ||
{ | { | ||
− | + | *freq = ((*(note + 1)) << 8) + (*(note + 2)); | |
uint8_t d1 = *(note + 3); | uint8_t d1 = *(note + 3); | ||
uint8_t d2 = *(note + 4); | uint8_t d2 = *(note + 4); | ||
− | + | *del = (music_speed * 16 * (long)d1) / d2; | |
− | |||
− | |||
− | |||
− | |||
} | } | ||
else if (*note == 254) | else if (*note == 254) | ||
{ | { | ||
− | + | *freq = FIS3; | |
− | + | *del = music_speed; | |
} | } | ||
else if (*note == 255) | else if (*note == 255) | ||
{ | { | ||
− | + | *freq = G3; | |
− | + | *del = music_speed; | |
} | } | ||
else | else | ||
{ | { | ||
uint8_t len = *note / 50; | uint8_t len = *note / 50; | ||
− | + | *del = music_speed; | |
− | while (len--) | + | while (len--) *del *= 2; |
uint8_t n = *note % 50; | uint8_t n = *note % 50; | ||
if (n != 49) | if (n != 49) | ||
Line 202: | Line 194: | ||
uint8_t octave = (n + 5) / 12; | uint8_t octave = (n + 5) / 12; | ||
n = (n + 5 ) % 12; | n = (n + 5 ) % 12; | ||
− | float | + | float ffreq = octave_4[n]; |
octave = 4 - octave; | octave = 4 - octave; | ||
while (octave > 0) | while (octave > 0) | ||
{ | { | ||
− | + | ffreq /= 2.0; | |
octave--; | octave--; | ||
} | } | ||
− | + | *freq = (uint16_t)ffreq; | |
} | } | ||
− | + | else *ffreq = 0; | |
− | |||
} | } | ||
+ | } | ||
+ | |||
+ | void otto_tone(uint8_t *note) | ||
+ | { | ||
+ | uint16_t freq, d; | ||
+ | otto_translate_tone(note, &freq, &d); | ||
+ | if (freq) tone(SIRENA, freq); else noTone(SIRENA); | ||
+ | delay(d); | ||
+ | noTone(SIRENA); | ||
} | } | ||
Line 230: | Line 230: | ||
delay(3000); | delay(3000); | ||
} | } | ||
− | |||
void test_ultrazvuk() | void test_ultrazvuk() | ||
Line 257: | Line 256: | ||
if (d >= 15) count2++; | if (d >= 15) count2++; | ||
else count2 = 0; | else count2 = 0; | ||
+ | if (serial_available() || Serial.available()) return; | ||
} | } | ||
if (millis() - tm >= 1500) | if (millis() - tm >= 1500) | ||
Line 299: | Line 299: | ||
void menu_command(int cmd) | void menu_command(int cmd) | ||
{ | { | ||
+ | if (cmd == 1) vpred(); | ||
if (cmd == 2) zahraj_melodiu(); | if (cmd == 2) zahraj_melodiu(); | ||
if (cmd == 3) melodia_jedna_druhej(); | if (cmd == 3) melodia_jedna_druhej(); | ||
− | |||
if (cmd == 4) ahoj(); | if (cmd == 4) ahoj(); | ||
serial_println_num(cmd); | serial_println_num(cmd); | ||
Line 329: | Line 329: | ||
} | } | ||
+ | // chodza pre vacsieho dreveneho robota | ||
uint16_t chor_time[] = {100,1,1,100,1,1,1,100,1,1,1,100,1,100,1,0}; | uint16_t chor_time[] = {100,1,1,100,1,1,1,100,1,1,1,100,1,100,1,0}; | ||
uint8_t chor_servo[] = {1,2,6,4,3,6,5,1,2,6,5,3,4,1,2,0}; | uint8_t chor_servo[] = {1,2,6,4,3,6,5,1,2,6,5,3,4,1,2,0}; | ||
uint8_t chor_val[] = {48,69,180,104,104,90,0,111,146,0,90,62,69,48,69,0}; | uint8_t chor_val[] = {48,69,180,104,104,90,0,111,146,0,90,62,69,48,69,0}; | ||
uint8_t chor_len = 16; | uint8_t chor_len = 16; | ||
− | |||
− | |||
void vpred() | void vpred() |
Revision as of 05:45, 9 August 2018
1 #include <Servo.h>
2 #include <EEPROM.h>
3
4 #define ECHO_BT_TO_USB 1
5
6 #define US_TRIG 7
7 #define US_ECHO 8
8
9 #define BT_RX 2
10 #define BT_TX 4
11
12 #define SIRENA 12
13
14 //maximalna dlzka choreografie
15 #define CHOREO_LEN 200
16
17 // cisla pinov, kde su zapojene servo motory
18 #define PIN_SERVO_LAVA_RUKA 10
19 #define PIN_SERVO_PRAVA_RUKA 11
20 #define PIN_SERVO_LAVA_NOHA 9
21 #define PIN_SERVO_PRAVA_NOHA 6
22 #define PIN_SERVO_LAVA_PATA 5
23 #define PIN_SERVO_PRAVA_PATA 3
24
25 #define S1 3
26 #define S2 5
27 #define S3 6
28 #define S4 9
29 #define S5 10
30 #define S6 11
31 #define SIRENA 12
32
33 //maximalna dlzka choreografie
34 #define CHOREO_LEN 200
35
36 // tu su serva cislovane 1-6
37 #define SERVO_LAVA_RUKA 5
38 #define SERVO_PRAVA_RUKA 6
39 #define SERVO_LAVA_NOHA 4
40 #define SERVO_PRAVA_NOHA 3
41 #define SERVO_LAVA_PATA 2
42 #define SERVO_PRAVA_PATA 1
43
44 // ak su niektore serva naopak, je tu jednotka
45 uint8_t servo_invertovane[6] = {0, 0, 1, 1, 0, 1};
46
47 // znaky, ktorymi sa ovladaju jednotlive stupne volnosti
48 char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' };
49 // co robia jednotlive znaky (znamienko urcuje smer)
50 int8_t zmeny[] = {SERVO_LAVA_RUKA, -SERVO_LAVA_RUKA,
51 SERVO_PRAVA_RUKA, -SERVO_PRAVA_RUKA,
52 -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA,
53 -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA,
54 SERVO_LAVA_PATA, -SERVO_LAVA_PATA,
55 SERVO_PRAVA_PATA, -SERVO_PRAVA_PATA };
56
57 // sem si mozno ulozit svoju kalibraciu
58 //uint8_t prednastavena_kalibracia[] = { 78, 69, 83, 80, 50, 67 };
59 uint8_t prednastavena_kalibracia[] = { 90, 90, 90, 90, 90, 90 };
60
61 uint8_t dolny_limit[] = {0, 0, 0, 0, 0, 0, 0, 0};
62 uint8_t horny_limit[] = {180, 180, 180, 180, 180, 180};
63
64 Servo s[6];
65
66 uint16_t ch_time[CHOREO_LEN];
67 uint8_t ch_servo[CHOREO_LEN];
68 uint8_t ch_val[CHOREO_LEN];
69 int ch_len;
70 uint8_t kalib[6];
71 int stav[6];
72 int krok;
73 uint8_t spomalenie;
74
75 static volatile int16_t distance;
76
77 uint8_t *melodia;
78 uint16_t dlzka_melodie;
79
80 #define FIS3 2960
81 #define G3 3136
82
83 float octave_4[] = { 2093.00, 2217.46, 2349.32, 2489.02, 2637.02, 2793.83, 2959.96, 3135.96, 3322.44, 3520.00, 3729.31, 3951.07 };
84 int16_t music_speed;
85
86 uint16_t dlzka_popcorn = 390;
87 uint8_t popcorn[] = { 149, 49,
88 28, 31, 35, 40, 49, 99, 38, 49, 99, 40, 49, 99, 35, 49, 99, 31, 49, 99, 35, 49, 99, 28, 49, 99, 49,
89 28, 31, 35, 40, 49, 99, 38, 49, 99, 40, 49, 99, 35, 49, 99, 31, 49, 99, 35, 49, 99, 28, 49, 99, 149,
90 40, 49, 99, 42, 49, 99, 43, 49, 99, 42, 49, 99, 43, 49, 99, 40, 49, 99, 42, 49, 99, 40, 49, 99, 42, 49, 99, 38, 49, 99, 40, 49, 99, 38, 49, 99, 40, 49, 99, 36, 49, 99, 40, 49, 99,
91 28, 31, 35, 40, 49, 99, 38, 49, 99, 40, 49, 99, 35, 49, 99, 31, 49, 99, 35, 49, 99, 28, 49, 99, 49,
92 28, 31, 35, 40, 49, 99, 38, 49, 99, 40, 49, 99, 35, 49, 99, 31, 49, 99, 35, 49, 99, 28, 49, 99, 149,
93 40, 49, 99, 42, 49, 99, 43, 49, 99, 42, 49, 99, 43, 49, 99, 40, 49, 99, 42, 49, 99, 40, 49, 99, 42, 49, 99, 38, 49, 99, 40, 49, 99, 38, 49, 99, 40, 49, 99, 42, 49, 99, 43, 49, 99,
94 49, 35, 38, 43, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 38, 49, 99, 43, 49, 99, 35, 49, 99,
95 49, 35, 38, 43, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 38, 49, 99, 43, 49, 99, 35, 49, 99, 149 ,
96 40, 49, 99, 47, 49, 99, 254, 49, 99, 255, 49, 99, 254, 49, 99, 255, 49, 99, 47, 49, 99, 254, 49, 99, 47, 49, 99, 254, 49, 99, 45, 49, 99, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 47, 49, 99,
97 49, 35, 38, 43, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 38, 49, 99, 43, 49, 99, 35, 49, 99,
98 49, 35, 38, 43, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 38, 49, 99, 43, 49, 99, 35, 49, 99, 149 ,
99 40, 49, 99, 47, 49, 99, 254, 49, 99, 255, 49, 99, 254, 49, 99, 255, 49, 99, 47, 49, 99, 254, 49, 99, 47, 49, 99, 254, 49, 99, 45, 49, 99, 47, 49, 99, 45, 49, 99, 47, 49, 99, 254, 49, 99, 255, 49, 99
100 };
101
102 void setup() {
103 Serial.begin(9600);
104 init_serial(9600);
105 init_ultrasonic();
106
107 melodia = popcorn;
108 dlzka_melodie = dlzka_popcorn;
109
110 randomSeed(analogRead(1));
111 s[0].attach(PIN_SERVO_PRAVA_PATA);
112 s[1].attach(PIN_SERVO_LAVA_PATA);
113 s[2].attach(PIN_SERVO_PRAVA_NOHA);
114 s[3].attach(PIN_SERVO_LAVA_NOHA);
115 s[4].attach(PIN_SERVO_LAVA_RUKA);
116 s[5].attach(PIN_SERVO_PRAVA_RUKA);
117 precitaj_kalibraciu_z_EEPROM();
118 for (int i = 0; i < 6; i++)
119 {
120 kalib[i] = prednastavena_kalibracia[i];
121 stav[i] = kalib[i];
122 s[i].write(stav[i]);
123 }
124 ch_len = 0;
125 krok = 7;
126 spomalenie = 6;
127 ahoj();
128 ruky2();
129 delay(100);
130 serial_println("\r\n Otto DTDT");
131 }
132
133 void loop() {
134 char z = -1;
135 if (serial_available()) z = serial_read();
136 #ifdef ECHO_BT_TO_USB
137 if (Serial.available()) z = Serial.read();
138 #endif
139
140 if (z != -1)
141 {
142 if (pohyb_znakom(z)) return;
143 else if (pohyb_kombinacia(z)) return;
144 else if (z == '@') nacitaj_choreografiu();
145 else if (z == '?') vypis_choreografiu();
146 else if (z == 't') zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
147 else if (z == '-') ahoj();
148 else if (z == ' ') reset();
149 else if (z == 'H') kalibruj();
150 else if (z == 'J') nastav_limity();
151 else if (z == 'G') vypis_kalibraciu();
152 else if (z == 'L') nacitaj_kalibraciu();
153 else if (z == 'E') zapis_kalibraciu_do_EEPROM();
154 else if (z == 'R') ruky();
155 else if (z == '9') zvys_krok();
156 else if (z == '1') zniz_krok();
157 else if (z == '8') zvys_spomalenie();
158 else if (z == '7') zniz_spomalenie();
159 else if (z == 'U') test_ultrazvuk();
160 else if (z == 'S') zobraz_stav();
161 }
162 int16_t d = measure_distance();
163 if (d < 10) menu_ultrasonic_request();
164 }
165
166 void otto_translate_tone(uint8_t *note, uint16_t *freq, uint16_t *del)
167 {
168 if (*note == 251)
169 {
170 *freq = ((*(note + 1)) << 8) + (*(note + 2));
171 uint8_t d1 = *(note + 3);
172 uint8_t d2 = *(note + 4);
173 *del = (music_speed * 16 * (long)d1) / d2;
174 }
175 else if (*note == 254)
176 {
177 *freq = FIS3;
178 *del = music_speed;
179 }
180 else if (*note == 255)
181 {
182 *freq = G3;
183 *del = music_speed;
184 }
185 else
186 {
187 uint8_t len = *note / 50;
188 *del = music_speed;
189 while (len--) *del *= 2;
190 uint8_t n = *note % 50;
191 if (n != 49)
192 {
193 uint8_t octave = (n + 5) / 12;
194 n = (n + 5 ) % 12;
195 float ffreq = octave_4[n];
196 octave = 4 - octave;
197 while (octave > 0)
198 {
199 ffreq /= 2.0;
200 octave--;
201 }
202 *freq = (uint16_t)ffreq;
203 }
204 else *ffreq = 0;
205 }
206 }
207
208 void otto_tone(uint8_t *note)
209 {
210 uint16_t freq, d;
211 otto_translate_tone(note, &freq, &d);
212 if (freq) tone(SIRENA, freq); else noTone(SIRENA);
213 delay(d);
214 noTone(SIRENA);
215 }
216
217 void zahraj_melodiu()
218 {
219 music_speed = 800 / 16;
220 for (int j = 0; j < 4; j++)
221 {
222 for (int i = 0; i < dlzka_melodie; i++)
223 {
224 otto_tone(melodia + i);
225 if (melodia[i] == 251) i += 4;
226 }
227 music_speed -= 150 / 16;
228 }
229 delay(3000);
230 }
231
232 void test_ultrazvuk()
233 {
234 int i = 0;
235 while ((serial_available() == 0) && (Serial.available() == 0))
236 {
237 serial_println_num(measure_distance());
238 delay(100);
239 }
240 serial_read();
241 }
242
243 void menu_ultrasonic_request()
244 {
245 uint32_t tm = millis();
246 int d = measure_distance();
247 int count = 0;
248 int count2 = 0;
249 while ((millis() - tm < 1500) && ((d < 15) || (count2 < 5)) && (count < 10))
250 {
251 delay(10);
252 d = measure_distance();
253 if (d == 10000) count++;
254 else count = 0;
255 if (d >= 15) count2++;
256 else count2 = 0;
257 if (serial_available() || Serial.available()) return;
258 }
259 if (millis() - tm >= 1500)
260 ultrasonic_menu();
261 }
262
263 void ultrasonic_menu()
264 {
265 int selection = 0;
266 tone(SIRENA, 880, 200);
267
268 do {
269 int count = 0;
270 do {
271 int32_t d = measure_distance();
272 if (d == 10000) continue;
273 if (d >= 20) count++;
274 else count = 0;
275 delay(10);
276 } while (!serial_available() && !Serial.available() && (count < 20));
277
278 tone(SIRENA, 440, 200);
279 uint32_t tm = millis();
280 while ((measure_distance() > 15) && (millis() - tm < 1500) && !serial_available() && !Serial.available()) delay(10);
281 if (millis() - tm >= 1500)
282 {
283 tone(SIRENA, 2000, 50);
284 menu_command(selection);
285 return;
286 }
287 selection++;
288 for (int i = 0; i < selection; i++)
289 {
290 tone(SIRENA, 1261, 50);
291 delay(250);
292 }
293 } while (!serial_available() && !Serial.available());
294 while (serial_available()) serial_read();
295 while (Serial.available()) Serial.read();
296 }
297
298 void menu_command(int cmd)
299 {
300 if (cmd == 1) vpred();
301 if (cmd == 2) zahraj_melodiu();
302 if (cmd == 3) melodia_jedna_druhej();
303 if (cmd == 4) ahoj();
304 serial_println_num(cmd);
305 }
306
307 void melodia_jedna_druhej()
308 {
309 for (int i = 0; i < 2; i++)
310 {
311 tone(SIRENA, 262, 200);
312 delay(200);
313 tone(SIRENA, 330, 200);
314 delay(200);
315
316 tone(SIRENA, 262, 200);
317 delay(200);
318 tone(SIRENA, 330, 200);
319 delay(200);
320
321 tone(SIRENA, 392, 400);
322 delay(400);
323
324 tone(SIRENA, 392, 400);
325 delay(400);
326 }
327 noTone(8);
328 }
329
330 // chodza pre vacsieho dreveneho robota
331 uint16_t chor_time[] = {100,1,1,100,1,1,1,100,1,1,1,100,1,100,1,0};
332 uint8_t chor_servo[] = {1,2,6,4,3,6,5,1,2,6,5,3,4,1,2,0};
333 uint8_t chor_val[] = {48,69,180,104,104,90,0,111,146,0,90,62,69,48,69,0};
334 uint8_t chor_len = 16;
335
336 void vpred()
337 {
338 while (measure_distance() > 30)
339 zatancuj_choreografiu(chor_time, chor_servo, chor_val, chor_len);
340 pipni();
341 reset();
342 }
343
344 void precitaj_kalibraciu_z_EEPROM()
345 {
346 uint8_t value = EEPROM.read(1);
347 if (value != '~') return;
348 for (int i = 2; i < 8; i++)
349 prednastavena_kalibracia[i - 2] = EEPROM.read(i);
350 for (int i = 0; i < 6; i++)
351 dolny_limit[i] = EEPROM.read(i + 9);
352 for (int i = 0; i < 6; i++)
353 horny_limit[i] = EEPROM.read(i + 15);
354 }
355
356 char read_char()
357 {
358 while (!serial_available() && !Serial.available());
359 if (serial_available()) return serial_read();
360 else return Serial.read();
361 }
362
363 void zapis_kalibraciu_do_EEPROM()
364 {
365 serial_print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
366 char odpoved = read_char();
367 serial_println_char(odpoved);
368 if (odpoved == 'Y')
369 {
370 EEPROM.write(1, '~');
371 for (int i = 2; i < 8; i++)
372 EEPROM.write(i, kalib[i - 2]);
373 for (int i = 0; i < 6; i++)
374 EEPROM.write(9 + i, dolny_limit[i]);
375 for (int i = 0; i < 6; i++)
376 EEPROM.write(15 + i, horny_limit[i]);
377 serial_println("ok");
378 }
379 }
380
381 void pipni()
382 {
383 tone(SIRENA, 1568, 50);
384 delay(100);
385 tone(SIRENA, 1357, 50);
386 }
387
388 void ruky()
389 {
390 int odloz_krok = krok;
391 delay(500);
392 krok = 90;
393 pohyb(SERVO_LAVA_RUKA);
394 pohyb(SERVO_PRAVA_RUKA);
395 delay(1000);
396 krok = 180;
397 pohyb(-SERVO_LAVA_RUKA);
398 pohyb(-SERVO_PRAVA_RUKA);
399 delay(1000);
400 krok = odloz_krok;
401 pipni();
402 }
403
404 void ruky2()
405 {
406 int odloz_krok = krok;
407 delay(500);
408 krok = 180;
409 pohyb(SERVO_LAVA_RUKA);
410 pohyb(SERVO_PRAVA_RUKA);
411 delay(1000);
412 krok = 90;
413 pohyb(-SERVO_LAVA_RUKA);
414 pohyb(-SERVO_PRAVA_RUKA);
415 delay(1000);
416 krok = odloz_krok;
417 pipni();
418 }
419
420 void ahoj()
421 {
422 tone(SIRENA, 1568, 50);
423 delay(70);
424 tone(SIRENA, 1175, 30);
425 delay(50);
426 tone(SIRENA, 880, 30);
427 delay(50);
428 tone(SIRENA, 1047, 50);
429 delay(70);
430 tone(SIRENA, 1245, 30);
431 delay(150);
432 tone(SIRENA, 1568, 50);
433 delay(100);
434 if (random(10) > 4) tone(SIRENA, 1357, 50);
435 else tone(SIRENA, 1047, 50);
436 }
437
438 void nastav_koncatinu(int8_t servo, uint8_t poloha)
439 {
440 int8_t delta;
441 int8_t srv = (servo > 0)?servo:-servo;
442 srv--;
443 poloha += kalib[srv] - 90;
444 if (poloha > 180) poloha = 180;
445 if (poloha < 0) poloha = 0;
446
447 if (stav[srv] < poloha) delta = 1;
448 else delta = -1;
449 while (stav[srv] != poloha)
450 {
451 stav[srv] +=delta;
452 s[srv].write(stav[srv]);
453 delay(spomalenie);
454 }
455 }
456
457 void zobraz_stav()
458 {
459 for (int i = 0; i < 6; i++)
460 {
461 serial_print("S"); serial_print_num(i + 1); serial_print(": "); serial_println_num(stav[i] - kalib[i] + 90);
462 }
463 serial_println("---");
464 pipni();
465 }
466
467 void pohyb(int8_t servo)
468 {
469 int8_t srv = (servo > 0)?servo:-servo;
470 srv--;
471 if (servo_invertovane[srv]) servo = -servo;
472 if (servo > 0)
473 {
474 if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
475 else stav[srv] = horny_limit[srv];
476 s[srv].write(stav[srv]);
477 }
478 else if (servo < 0)
479 {
480 if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok;
481 else stav[srv] = dolny_limit[srv];
482 s[srv].write(stav[srv]);
483 }
484 }
485
486 uint8_t pohyb_znakom(char z)
487 {
488 for (int i = 0; i < 12; i++)
489 {
490 if (z == znaky_zmien[i])
491 {
492 int8_t servo = zmeny[i];
493 pohyb(servo);
494 }
495 }
496 }
497
498 void kombinacia1()
499 {
500 pohyb(SERVO_LAVA_NOHA);
501 pohyb(-SERVO_PRAVA_PATA);
502 }
503
504 void kombinacia2()
505 {
506 pohyb(SERVO_PRAVA_NOHA);
507 pohyb(-SERVO_LAVA_PATA);
508 }
509
510 void kombinacia3()
511 {
512 pohyb(SERVO_LAVA_RUKA);
513 pohyb(SERVO_PRAVA_RUKA);
514 }
515
516 void kombinacia4()
517 {
518 pohyb(-SERVO_LAVA_RUKA);
519 pohyb(-SERVO_PRAVA_RUKA);
520 }
521
522 int pohyb_kombinacia(char z)
523 {
524 if (z == '3') kombinacia1();
525 else if (z == '4') kombinacia2();
526 else if (z == '5') kombinacia3();
527 else if (z == '6') kombinacia4();
528 else return 0;
529 return 1;
530 }
531
532 int nacitajCislo()
533 {
534 int num = 0;
535 int z;
536 do {
537 z = read_char();
538 if (z == '#') while (z != 13) z = read_char();
539 } while ((z < '0') || (z > '9'));
540 while ((z >= '0') && (z <= '9'))
541 {
542 num *= 10;
543 num += (z - '0');
544 do { z = read_char(); if (z == -1) delayMicroseconds(10); } while (z < 0);
545 }
546 return num;
547 }
548
549 void nacitaj_choreografiu()
550 {
551 ch_len = 0;
552 int tm;
553 do {
554 tm = nacitajCislo();
555 ch_time[ch_len] = tm;
556 ch_servo[ch_len] = nacitajCislo();
557 ch_val[ch_len] = nacitajCislo();
558 ch_len++;
559 if (ch_len == CHOREO_LEN) break;
560 } while (tm > 0);
561 pipni();
562 }
563
564 void vypis_choreografiu()
565 {
566 for (int i = 0; i < ch_len; i++)
567 {
568 serial_print_num(ch_time[i]);
569 serial_print(" ");
570 serial_print_num(ch_servo[i]);
571 serial_print(" ");
572 serial_println_num(ch_val[i]);
573 }
574 pipni();
575 }
576
577 void zatancuj_choreografiu(uint16_t *ch_time, uint8_t *ch_servo, uint8_t *ch_val, int ch_len )
578 {
579 for (int i = 0; i < ch_len - 1; i++)
580 {
581 delay(ch_time[i]);
582 nastav_koncatinu(ch_servo[i], ch_val[i]);
583 }
584 // if (ch_len > 0) stav[ch_servo[ch_len - 1]] = ch_val[ch_len - 1];
585 pipni();
586 while (serial_available()) serial_read();
587 while (Serial.available()) Serial.read();
588 }
589
590 void reset()
591 {
592 for (int i = 0; i < 6; i++)
593 {
594 stav[i] = kalib[i];
595 s[i].write(kalib[i]);
596 delay(300);
597 }
598 pipni();
599 }
600
601 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
602 {
603 serial_print(" (+/-/ENTER): ");
604 serial_println_num(hodnota);
605 s[servo].write(hodnota);
606 char z;
607 do {
608 z = read_char();
609 if ((z == '+') && (hodnota < 180)) hodnota++;
610 else if ((z == '-') && (hodnota > 0)) hodnota--;
611 if ((z == '+') || (z == '-'))
612 {
613 serial_print_num(hodnota); serial_print_char('\r');
614 s[servo].write(hodnota);
615 }
616 } while (z != 13);
617 return hodnota;
618 }
619
620 void kalibruj()
621 {
622 for (int i = 0; i < 6; i++)
623 {
624 serial_print_num(i);
625 kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
626 serial_print_num(i);
627 serial_print(": ");
628 serial_println_num(kalib[i]);
629 }
630 for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
631 serial_println("ok");
632 pipni();
633 }
634
635 void nastav_limity()
636 {
637 for (int i = 0; i < 6; i++)
638 {
639 serial_print_num(i);
640 serial_print("dolny");
641 dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
642 serial_print_num(i);
643 serial_print(" dolny: ");
644 serial_println_num(dolny_limit[i]);
645 s[i].write(kalib[i]);
646
647 serial_print_num(i);
648 serial_print("horny");
649 horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
650 serial_print_num(i);
651 serial_print(" horny: ");
652 serial_println_num(horny_limit[i]);
653 s[i].write(kalib[i]);
654 }
655 for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print("-"); serial_print_num(horny_limit[i]); serial_print(" "); }
656 serial_println("ok");
657 pipni();
658 }
659
660 void vypis_kalibraciu()
661 {
662 serial_print("stredy: ");
663 for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
664 serial_println();
665 serial_print("dolny limit: ");
666 for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print(" "); }
667 serial_println();
668 serial_print("horny limit: ");
669 for (int i = 0; i < 6; i++) { serial_print_num(horny_limit[i]); serial_print(" "); }
670 serial_println();
671 }
672
673 void nacitaj_kalibraciu()
674 {
675 int tm;
676 for (int i = 0; i < 6; i++)
677 kalib[i] = nacitajCislo();
678 vypis_kalibraciu();
679 serial_println("ok");
680 pipni();
681 }
682
683 void zvys_krok()
684 {
685 if (krok < 180) krok++;
686 serial_print("krok: ");
687 serial_println_num(krok);
688 }
689
690 void zniz_krok()
691 {
692 if (krok > 0) krok--;
693 serial_print("krok: ");
694 serial_println_num(krok);
695 }
696
697 void zniz_spomalenie()
698 {
699 if (spomalenie > 0) spomalenie--;
700 serial_print("spomalenie: ");
701 serial_println_num(spomalenie);
702 }
703
704 void zvys_spomalenie()
705 {
706 if (spomalenie < 100) spomalenie++;
707 serial_print("spomalenie: ");
708 serial_println_num(spomalenie);
709 }
710
711 // nasleduje softverova implementacia serioveho portu
712 #define SERIAL_STATE_IDLE 0
713 #define SERIAL_STATE_RECEIVING 1
714 #define SERIAL_BUFFER_LENGTH 20
715
716 static volatile uint8_t serial_state;
717 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
718 static volatile uint8_t serial_buf_wp, serial_buf_rp;
719
720 static volatile uint8_t receiving_byte;
721
722 static volatile uint32_t time_startbit_noticed;
723 static volatile uint8_t next_bit_order;
724 static volatile uint8_t waiting_stop_bit;
725 static uint16_t one_byte_duration;
726 static uint16_t one_bit_duration;
727 static uint16_t one_bit_write_duration;
728 static uint16_t half_of_one_bit_duration;
729
730 void init_serial(uint32_t baud_rate)
731 {
732 pinMode(2, INPUT);
733 pinMode(4, OUTPUT);
734
735 serial_state = SERIAL_STATE_IDLE;
736
737 one_byte_duration = 9500000 / baud_rate;
738 one_bit_duration = 1000000 / baud_rate;
739 one_bit_write_duration = one_bit_duration - 1;
740 half_of_one_bit_duration = 500000 / baud_rate;
741
742 PCMSK2 |= 4; //PCINT18;
743 PCIFR &= ~4; //PCIF2;
744 PCICR |= 4; // PCIE2;
745 }
746
747 ISR(PCINT2_vect)
748 {
749 uint32_t tm = micros();
750 if (serial_state == SERIAL_STATE_IDLE)
751 {
752 time_startbit_noticed = tm;
753 serial_state = SERIAL_STATE_RECEIVING;
754 receiving_byte = 0xFF;
755 next_bit_order = 0;
756 }
757 else if (tm - time_startbit_noticed > one_byte_duration)
758 {
759 serial_buffer[serial_buf_wp] = receiving_byte;
760 serial_buf_wp++;
761 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
762 time_startbit_noticed = tm;
763 receiving_byte = 0xFF;
764 next_bit_order = 0;
765 }
766 else if (PIND & 4)
767 {
768 int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
769 while (next_bit_order < new_next_bit_order)
770 {
771 receiving_byte &= ~(1 << next_bit_order);
772 next_bit_order++;
773 }
774 if (next_bit_order == 8)
775 {
776 serial_buffer[serial_buf_wp] = receiving_byte;
777 serial_buf_wp++;
778 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
779 serial_state = SERIAL_STATE_IDLE;
780 }
781 } else
782 next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
783 }
784
785 uint8_t serial_available()
786 {
787 cli();
788 if (serial_buf_rp != serial_buf_wp)
789 {
790 sei();
791 return 1;
792 }
793 if (serial_state == SERIAL_STATE_RECEIVING)
794 {
795 uint32_t tm = micros();
796 if (tm - time_startbit_noticed > one_byte_duration)
797 {
798 serial_state = SERIAL_STATE_IDLE;
799 serial_buffer[serial_buf_wp] = receiving_byte;
800 serial_buf_wp++;
801 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
802 sei();
803 return 1;
804 }
805 }
806 sei();
807 return 0;
808 }
809
810 int16_t serial_read()
811 {
812 cli();
813 if (serial_buf_rp != serial_buf_wp)
814 {
815 uint8_t ch = serial_buffer[serial_buf_rp];
816 serial_buf_rp++;
817 if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
818 sei();
819 return ch;
820 }
821
822 if (serial_state == SERIAL_STATE_RECEIVING)
823 {
824 uint32_t tm = micros();
825 if (tm - time_startbit_noticed > one_byte_duration)
826 {
827 uint8_t ch = receiving_byte;
828 serial_state = SERIAL_STATE_IDLE;
829 sei();
830 return ch;
831 }
832 }
833 sei();
834 return -1;
835 }
836
837 void serial_write(uint8_t ch)
838 {
839 #ifdef ECHO_BT_TO_USB
840 Serial.print((char)ch);
841 #endif
842 PORTD &= ~16;
843 delayMicroseconds(one_bit_write_duration);
844 for (uint8_t i = 0; i < 8; i++)
845 {
846 if (ch & 1) PORTD |= 16;
847 else PORTD &= ~16;
848 ch >>= 1;
849 delayMicroseconds(one_bit_write_duration);
850 }
851 PORTD |= 16;
852 delayMicroseconds(one_bit_write_duration);
853 delayMicroseconds(one_bit_write_duration);
854 delayMicroseconds(one_bit_write_duration);
855 delayMicroseconds(one_bit_write_duration);
856 delayMicroseconds(one_bit_write_duration);
857 }
858
859 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
860 {
861 uint16_t len;
862 uint16_t ch;
863 do {
864 ch = serial_read();
865 if (ch == 13) continue;
866 } while (ch == -1);
867
868 do {
869 if ((ch != 13) && (ch != 10) && (ch != -1))
870 {
871 *(ln++) = ch;
872 max_length--;
873 len++;
874 }
875 ch = serial_read();
876 } while ((ch != 13) && max_length);
877 *ln = 0;
878 return len;
879 }
880
881 void serial_print_num(int32_t number)
882 {
883 if (number < 0)
884 {
885 serial_write('-');
886 number = -number;
887 }
888 int32_t rad = 1;
889 while (number / rad) rad *= 10;
890 if (number > 0) rad /= 10;
891 while (rad)
892 {
893 serial_write((char)('0' + (number / rad)));
894 number -= (number / rad) * rad;
895 rad /= 10;
896 }
897 }
898
899 void serial_print_char(char ch)
900 {
901 serial_write(ch);
902 }
903
904 void serial_print(const uint8_t *str)
905 {
906 while (*str) serial_write(*(str++));
907 }
908
909 void serial_println(const uint8_t *str)
910 {
911 serial_print(str);
912 serial_write(13);
913 serial_write(10);
914 }
915
916 void serial_println_num(int32_t number)
917 {
918 serial_print_num(number);
919 serial_println();
920 }
921
922 void serial_println_char(char ch)
923 {
924 serial_write(ch);
925 serial_println();
926 }
927
928 void serial_println()
929 {
930 serial_write(13);
931 serial_write(10);
932 }
933
934 // nasleduje citanie z utltazvukoveho senzora
935
936 static volatile uint32_t pulse_start;
937 static volatile uint8_t new_distance;
938
939 void init_ultrasonic()
940 {
941 pinMode(US_ECHO, INPUT);
942 pinMode(US_TRIG, OUTPUT);
943
944 PCMSK0 |= 1; //PCINT0;
945 PCIFR &= ~1; //PCIF0;
946 PCICR |= 1; // PCIE0;
947 }
948
949 ISR(PCINT0_vect)
950 {
951 if (PINB & 1) pulse_start = micros();
952 else
953 {
954 distance = (int16_t)((micros() - pulse_start) / 58);
955 new_distance = 1;
956 }
957 }
958
959 void start_distance_measurement()
960 {
961 distance = 10000;
962 new_distance = 0;
963 digitalWrite(US_TRIG, HIGH);
964 delayMicroseconds(10);
965 digitalWrite(US_TRIG, LOW);
966 }
967
968 void wait_for_distance_measurement_to_complete()
969 {
970 uint8_t counter = 0;
971 while ((counter < 20) && !new_distance)
972 {
973 delay(1);
974 counter++;
975 }
976 if (counter == 20)
977 {
978 pinMode(US_ECHO, OUTPUT);
979 digitalWrite(US_ECHO, HIGH);
980 delayMicroseconds(10);
981 digitalWrite(US_ECHO, LOW);
982 pinMode(US_ECHO, INPUT);
983 delayMicroseconds(5);
984 distance = 10000;
985 }
986 }
987
988 int16_t measure_distance()
989 {
990 start_distance_measurement();
991 wait_for_distance_measurement_to_complete();
992 return distance;
993 }