Difference between revisions of "Otto - riadiaci program v.4"
m |
m |
||
(9 intermediate revisions by 2 users not shown) | |||
Line 56: | Line 56: | ||
|- | |- | ||
|} | |} | ||
+ | |||
+ | Spojazdenie robota: | ||
+ | |||
+ | # ešte raz dôkladne skontroluj, či máš všetky kábliky pripojené správne - servokábliky musia byť otočené tak, že hnedý ide na zem (G)! | ||
+ | # robota budeš mať zatiaľ vypnutého (vypínač je OFF), pri konfigurácii sa bude napájať z USB | ||
+ | # downloadni si [https://www.chiark.greenend.org.uk/~sgtatham/putty/latest.html PUTTY], ak ho ešte nemáš | ||
+ | # downloadni si [http://arduino.cc Arduino IDE], ak ho ešte nemáš | ||
+ | # v Arduino IDE otvor program, ktorý je zalinkovaný nižšie (ottoX.ino) | ||
+ | # pripoj Arduino cez USB káblik k PC a v Device Manageri pohľadaj, ktorý sériový port sa vytvoril (CH340) - napr. COM3. (tieto tri kroky odporúčam robiť s Arduinom, kým nie je pripojené do rozširujúcej dosky) | ||
+ | # nastav port (Tools-Port) podľa predchádzajúceho bodu a Arduino Nano (Tools-Board) | ||
+ | # uploadni program do robota - ak to nejde, skús vybrať Arduino z dosky a uploadnuť program do voľného nepripojeného Arduina - ak to ide, tak si na doske niečo zle pripojil, ak to nejde, tak je buď Arduino už pokazené, alebo si vybral nesprávny port, alebo dosku, alebo USB káblik nemáš dobre pripojený | ||
+ | # po úspešnom uploade sa robot ozve (ak sa neozve, sirénka nie je dobre pripojená) | ||
+ | # otvor putty, vyber "serial", zadaj názov portu (napr. COM3) a nastav 9600 BPS, pripoj sa - robot by sa mal znova ozvať, lebo Arduino sa pri otváraní sériového portu vždy resetne | ||
+ | # stlač kláves '-' a robot by mal písknuť | ||
+ | # podrž klávesy 'q', 'a' - mal by hýbať rukou | ||
+ | # stlač kláves 'X' na preformátovanie tabuľky v EEPROM (na všetky potvrdenia y/n záleží na tom, či je malé velké písmeno - čiže potvrdzuješ iba veľkým 'Y' //sorry, malo tam byť skôr 'A' :) | ||
+ | # stlač kláves 'H' a postupuj podľa pokynov pri kalibrácii - vždy nastavuješ neutrálnu polohu - upažené ruky, zarovnané nohy, šľapy sú vodorovne na zemi | ||
+ | # stlač kláves 'W' na uloženie kalibrácie do EEPROM | ||
+ | # odpoj USB | ||
+ | # info: keď budeš mať otvorené putty okno cez USB, tak cez Arduino nejde programovať - najskôr treba zavrieť okno Putty (pre BT to neplatí, BT okno môže ostať otvorené počas programovania, programovať sa dá len cez USB) | ||
+ | # zapni robota, mal by sa ozvať a dať do nakalibrovanej polohy | ||
+ | # zapni si BT na notebooku a vyhľadaj zariadenia BlueTooth - pripoj HC05, párovací kód je 1234 | ||
+ | # v nastaveniach BT si zisti na aký port (výstupný/outgoing) ti HC05 pripojilo | ||
+ | # otvor si putty a nastav serial, tento port a 9600 | ||
+ | # gratulujem, veselú zábavu... (pozri si choreografie, ich formát, vytvor si nové pesničky, pripoj si ho cez android - vysielaj znaky 3,4,5,6 - bude chodiť rôznymi smermi) | ||
+ | # robot sám od seba rád škrieka, keď sa nudí, tak odporúčam vypínať režim nudy (alebo si to zmeň v programe), keby niečo nešlo, napíš Paľovi - ppetrovic@acm.org | ||
{|class="wikitable" | {|class="wikitable" | ||
− | ! | + | !kláves!!servo/funkcia |
|- | |- | ||
|a/q || ľavá ruka | |a/q || ľavá ruka | ||
Line 194: | Line 220: | ||
uint8_t auto_start; | uint8_t auto_start; | ||
− | + | volatile int16_t distance; | |
− | + | uint32_t cas_ked_naposledy_stlacil; | |
− | + | uint8_t vypni_nudu; | |
void setup() { | void setup() { | ||
Line 225: | Line 251: | ||
ruky(); | ruky(); | ||
delay(100); | delay(100); | ||
− | serial_println_flash(PSTR("\r\n Otto DTDT")); | + | serial_println_flash(PSTR("\r\n Otto DTDT, verzia 2018/09/14")); |
if (auto_start > 0) | if (auto_start > 0) | ||
{ | { | ||
Line 348: | Line 374: | ||
else if (z == '/') zahraj_melodiu(3); | else if (z == '/') zahraj_melodiu(3); | ||
else if (z == '+') zahraj_melodiu(4); | else if (z == '+') zahraj_melodiu(4); | ||
+ | else if (z == '!') zahraj_melodiu(5); | ||
else if (z == 27) zastav_melodiu(); | else if (z == 27) zastav_melodiu(); | ||
else return 0; | else return 0; | ||
Line 672: | Line 699: | ||
if (cmd == 9) zahraj_melodiu(3); | if (cmd == 9) zahraj_melodiu(3); | ||
if (cmd == 10) zahraj_melodiu(4); | if (cmd == 10) zahraj_melodiu(4); | ||
+ | if (cmd == 11) zahraj_melodiu(5); | ||
} | } | ||
Line 701: | Line 729: | ||
uint8_t chor_val[] = {1, 48, 69, 180, 104, 104, 90, 0, 111, 146, 0, 90, 62, 69, 48, 69, 2, 0}; | uint8_t chor_val[] = {1, 48, 69, 180, 104, 104, 90, 0, 111, 146, 0, 90, 62, 69, 48, 69, 2, 0}; | ||
uint8_t chor_len = 18; | uint8_t chor_len = 18; | ||
+ | |||
+ | uint16_t fwd_time[] = {1,1,100,1,1,1,100,1,1,1,100,1,100,1,1,100,1,100,1,1,1,0}; | ||
+ | uint8_t fwd_servo[] = {10,11,1,2,1,6,4,3,6,5,1,2,2,6,5,3,4,1,2,1,9,0}; | ||
+ | uint8_t fwd_val[] = {6,1,36,66,90,180,120,120,90,0,109,156,90,0,90,60,60,36,70,90,6,0}; | ||
+ | uint8_t fwd_len = 22; | ||
+ | |||
+ | uint8_t bwd_val[] = {6,2,36,66,90,180,60,60,90,0,109,156,90,0,90,120,120,36,70,90,6,0}; | ||
+ | uint8_t lt_val[] = {6,5,36,66,90,180,120,80,90,0,111,156,90,0,90,110,80,36,70,90,6,0}; | ||
+ | uint8_t rt_val[] = {6,4,36,66,90,180,80,120,90,0,109,156,90,0,90,80,110,36,70,90,6,0}; | ||
+ | |||
void vpred() | void vpred() | ||
{ | { | ||
− | while ((measure_distance() > | + | while ((measure_distance() > 10) && !serial_available() && !Serial.available()) |
zatancuj_choreografiu(chor_time, chor_servo, chor_val, chor_len); | zatancuj_choreografiu(chor_time, chor_servo, chor_val, chor_len); | ||
pipni(); | pipni(); | ||
Line 710: | Line 748: | ||
} | } | ||
+ | void chod_smerom(int s) | ||
+ | { | ||
+ | if (s == 1) zatancuj_choreografiu(fwd_time, fwd_servo, fwd_val, fwd_len); | ||
+ | else if (s == 2) zatancuj_choreografiu(fwd_time, fwd_servo, bwd_val, fwd_len); | ||
+ | else if (s == 3) zatancuj_choreografiu(fwd_time, fwd_servo, rt_val, fwd_len); | ||
+ | else if (s == 4) zatancuj_choreografiu(fwd_time, fwd_servo, lt_val, fwd_len); | ||
+ | } | ||
void pipni() | void pipni() | ||
{ | { | ||
Line 827: | Line 872: | ||
void kombinacia1() | void kombinacia1() | ||
{ | { | ||
− | pohyb(SERVO_LAVA_NOHA); | + | chod_smerom(1); |
− | + | // pohyb(SERVO_LAVA_NOHA); | |
+ | // pohyb(-SERVO_PRAVA_PATA); | ||
} | } | ||
void kombinacia2() | void kombinacia2() | ||
{ | { | ||
− | pohyb(SERVO_PRAVA_NOHA); | + | chod_smerom(2); |
− | + | ||
+ | // pohyb(SERVO_PRAVA_NOHA); | ||
+ | // pohyb(-SERVO_LAVA_PATA); | ||
} | } | ||
void kombinacia3() | void kombinacia3() | ||
{ | { | ||
− | pohyb(SERVO_LAVA_RUKA); | + | chod_smerom(3); |
− | + | ||
+ | // pohyb(SERVO_LAVA_RUKA); | ||
+ | // pohyb(SERVO_PRAVA_RUKA); | ||
} | } | ||
void kombinacia4() | void kombinacia4() | ||
{ | { | ||
− | + | chod_smerom(4); | |
− | + | ||
+ | // pohyb(-SERVO_LAVA_RUKA); | ||
+ | // pohyb(-SERVO_PRAVA_RUKA); | ||
} | } | ||
Line 937: | Line 989: | ||
{ | { | ||
if (serial_available() || Serial.available()) return len - 1; | if (serial_available() || Serial.available()) return len - 1; | ||
− | if (measure_distance() < | + | if (measure_distance() < 10) return len - 1; |
return ((int)argument) - 1; | return ((int)argument) - 1; | ||
} | } | ||
Line 1,149: | Line 1,201: | ||
} | } | ||
− | //nasleduju funkcie ktore pracuju s | + | //nasleduju funkcie ktore pracuju s pamatou EEPROM |
//EEPROM MAP: | //EEPROM MAP: | ||
Line 1,679: | Line 1,731: | ||
//popcorn | //popcorn | ||
− | uint16_t dlzka_melodia[] = {0, 386, 26, 281, 217}; | + | uint16_t dlzka_melodia[] = {0, 386, 26, 281, 217, 36}; |
const uint8_t melodia1[] PROGMEM = { 252, 50, 149, 49, | const uint8_t melodia1[] PROGMEM = { 252, 50, 149, 49, | ||
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, | 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, | ||
Line 1,727: | Line 1,779: | ||
99, 83, 81, 80, | 99, 83, 81, 80, | ||
185, 78, 80, 81, 83, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 78, 81, 78, 73, 81, 178 | 185, 78, 80, 81, 83, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 78, 81, 78, 73, 81, 178 | ||
+ | }; | ||
+ | |||
+ | //let it be https://www.musicnotes.com/sheetmusic/mtd.asp?ppn=MN0101556 | ||
+ | const uint8_t melodia5[] PROGMEM = { 252, 200, 26, 26, 76, 26, 253, 78, 73, 76, 76, 31, 253, 83, 35, 85, 253, 85, | ||
+ | 83, 83, 81, 131, 35, 253, 85, 86, 35, 35, 85, 83, 99, 35, 33, 83, 81, 181 | ||
}; | }; | ||
volatile int16_t music_speed = 800 / 16; | volatile int16_t music_speed = 800 / 16; | ||
− | |||
− | |||
volatile const uint8_t *current_note ; | volatile const uint8_t *current_note ; | ||
volatile uint16_t notes_remaining; | volatile uint16_t notes_remaining; | ||
+ | uint8_t dotted_note = 0; | ||
void zahraj_melodiu(uint8_t cislo) | void zahraj_melodiu(uint8_t cislo) | ||
Line 1,746: | Line 1,802: | ||
else if (cislo == 3) current_note = melodia3; | else if (cislo == 3) current_note = melodia3; | ||
else if (cislo == 4) current_note = melodia4; | else if (cislo == 4) current_note = melodia4; | ||
+ | else if (cislo == 5) current_note = melodia5; | ||
notes_remaining = dlzka_melodia[cislo]; | notes_remaining = dlzka_melodia[cislo]; | ||
Line 1,753: | Line 1,810: | ||
void next_note() | void next_note() | ||
{ | { | ||
− | uint16_t freq, dur; | + | uint16_t freq = 0, dur = 0; |
if (!notes_remaining) return; | if (!notes_remaining) return; | ||
otto_translate_tone_flash(&freq, &dur); | otto_translate_tone_flash(&freq, &dur); | ||
Line 1,763: | Line 1,820: | ||
do { | do { | ||
uint8_t n = pgm_read_byte(current_note); | uint8_t n = pgm_read_byte(current_note); | ||
− | if (n | + | if (n > 249) |
{ | { | ||
− | current_note++; | + | if (n == 251) |
− | + | { | |
− | + | current_note++; | |
− | + | uint8_t f1 = pgm_read_byte(current_note); | |
− | + | current_note++; | |
− | + | uint8_t f2 = pgm_read_byte(current_note); | |
− | + | current_note++; | |
− | + | uint8_t d1 = pgm_read_byte(current_note); | |
− | + | current_note++; | |
− | + | uint8_t d2 = pgm_read_byte(current_note); | |
− | + | *freq = (f1 << 8) + f2; | |
− | + | *del = (music_speed * 16 * (long)d1) / d2; | |
− | + | notes_remaining -= 4; | |
− | + | } | |
− | + | else if (n == 252) | |
− | + | { | |
− | + | current_note++; | |
− | + | music_speed = pgm_read_byte(current_note); | |
− | continue; | + | current_note++; |
− | + | notes_remaining -= 2; | |
− | + | continue; | |
− | + | } | |
− | + | else if (n == 253) | |
− | + | { | |
− | + | dotted_note = 1; | |
− | + | current_note++; | |
− | + | notes_remaining--; | |
− | + | continue; | |
− | + | } | |
+ | else if (n == 254) | ||
+ | { | ||
+ | *freq = FIS3; | ||
+ | *del = music_speed; | ||
+ | } | ||
+ | else if (n == 255) | ||
+ | { | ||
+ | *freq = G3; | ||
+ | *del = music_speed; | ||
+ | } | ||
} | } | ||
else | else | ||
Line 1,820: | Line 1,887: | ||
break; | break; | ||
} while (1); | } while (1); | ||
+ | if (dotted_note) | ||
+ | { | ||
+ | *del += (*del) / 2; | ||
+ | dotted_note = 0; | ||
+ | } | ||
} | } | ||
Line 1,830: | Line 1,902: | ||
notes_remaining = 0; | notes_remaining = 0; | ||
tone2_pause = 0; | tone2_pause = 0; | ||
+ | dotted_note = 0; | ||
TCCR2A = 2; | TCCR2A = 2; | ||
TCCR2B = 0; | TCCR2B = 0; | ||
Line 1,904: | Line 1,977: | ||
tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period; | tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period; | ||
if (tone2_len == 0) tone2_len++; | if (tone2_len == 0) tone2_len++; | ||
− | + | ||
+ | TCNT2 = 0; | ||
OCR2A = (uint8_t) (7813 / (uint32_t)freq); | OCR2A = (uint8_t) (7813 / (uint32_t)freq); | ||
TCCR2B = 7; | TCCR2B = 7; |
Latest revision as of 12:36, 21 November 2018
Program umožňuje priame riadenie robota (aj cez BlueTooth). Teraz už funguje aj na pinoch 2,4 a môže sa naraz programovať aj komunikovať.
Na komunikáciu s robotom môžete použiť napr. program Putty: putty.exe.
Pripojenie na vývody Arduina:
pin | signál |
---|---|
10 | ľavá ruka |
11 | pravá ruka |
9 | ľavá noha |
6 | pravá noha |
5 | ľavá päta |
3 | pravá päta |
2 | TXD BlueTooth |
4 | RXD BlueTooth |
7 | TRIG Ultrazvuk |
8 | ECHO Ultrazvuk |
12 | Siréna + |
G | Siréna -, GND Ultrazvuk, GND BlueTooth, kondenzátor -, najtmavší káblik každého serva, oba kábliky z vypínača |
V | VCC Ultrazvuk, VCC BlueTooth, kondenzátor +, červený káblik každého serva, oba kabliky z diody |
Číslovanie serva v choreografii:
servo | číslo serva v choreografii |
---|---|
ľavá ruka | 5 |
pravá ruka | 6 |
ľavá noha | 4 |
pravá noha | 3 |
ľavá päta | 2 |
pravá päta | 1 |
Spojazdenie robota:
- ešte raz dôkladne skontroluj, či máš všetky kábliky pripojené správne - servokábliky musia byť otočené tak, že hnedý ide na zem (G)!
- robota budeš mať zatiaľ vypnutého (vypínač je OFF), pri konfigurácii sa bude napájať z USB
- downloadni si PUTTY, ak ho ešte nemáš
- downloadni si Arduino IDE, ak ho ešte nemáš
- v Arduino IDE otvor program, ktorý je zalinkovaný nižšie (ottoX.ino)
- pripoj Arduino cez USB káblik k PC a v Device Manageri pohľadaj, ktorý sériový port sa vytvoril (CH340) - napr. COM3. (tieto tri kroky odporúčam robiť s Arduinom, kým nie je pripojené do rozširujúcej dosky)
- nastav port (Tools-Port) podľa predchádzajúceho bodu a Arduino Nano (Tools-Board)
- uploadni program do robota - ak to nejde, skús vybrať Arduino z dosky a uploadnuť program do voľného nepripojeného Arduina - ak to ide, tak si na doske niečo zle pripojil, ak to nejde, tak je buď Arduino už pokazené, alebo si vybral nesprávny port, alebo dosku, alebo USB káblik nemáš dobre pripojený
- po úspešnom uploade sa robot ozve (ak sa neozve, sirénka nie je dobre pripojená)
- otvor putty, vyber "serial", zadaj názov portu (napr. COM3) a nastav 9600 BPS, pripoj sa - robot by sa mal znova ozvať, lebo Arduino sa pri otváraní sériového portu vždy resetne
- stlač kláves '-' a robot by mal písknuť
- podrž klávesy 'q', 'a' - mal by hýbať rukou
- stlač kláves 'X' na preformátovanie tabuľky v EEPROM (na všetky potvrdenia y/n záleží na tom, či je malé velké písmeno - čiže potvrdzuješ iba veľkým 'Y' //sorry, malo tam byť skôr 'A' :)
- stlač kláves 'H' a postupuj podľa pokynov pri kalibrácii - vždy nastavuješ neutrálnu polohu - upažené ruky, zarovnané nohy, šľapy sú vodorovne na zemi
- stlač kláves 'W' na uloženie kalibrácie do EEPROM
- odpoj USB
- info: keď budeš mať otvorené putty okno cez USB, tak cez Arduino nejde programovať - najskôr treba zavrieť okno Putty (pre BT to neplatí, BT okno môže ostať otvorené počas programovania, programovať sa dá len cez USB)
- zapni robota, mal by sa ozvať a dať do nakalibrovanej polohy
- zapni si BT na notebooku a vyhľadaj zariadenia BlueTooth - pripoj HC05, párovací kód je 1234
- v nastaveniach BT si zisti na aký port (výstupný/outgoing) ti HC05 pripojilo
- otvor si putty a nastav serial, tento port a 9600
- gratulujem, veselú zábavu... (pozri si choreografie, ich formát, vytvor si nové pesničky, pripoj si ho cez android - vysielaj znaky 3,4,5,6 - bude chodiť rôznymi smermi)
- robot sám od seba rád škrieka, keď sa nudí, tak odporúčam vypínať režim nudy (alebo si to zmeň v programe), keby niečo nešlo, napíš Paľovi - ppetrovic@acm.org
kláves | servo/funkcia |
---|---|
a/q | ľavá ruka |
;/p | pravá ruka |
z/x | ľavá noha |
,/. | pravá noha |
d/c | ľavá päta |
k/m | pravá päta |
1/9 | zníž/zvýš rýchlosť pohybu |
H | kalibrovať strednú polohu servomotorov
|
J | kalibrovať limity pre všetky stupne voľnosti (len pre priame riadenie) |
G | zobrazenie celej kalibrácie |
E | celú kalibráciu je možné zapísať do trvalej pamäte EEPROM (používať opatrne! - dá sa prepísať najviac 100000-krát), po zapnutí sa automaticky načíta, treba potvrdiť veľkým Y |
minus | zvukový pozdrav |
R | ruky hore-dole |
@ | načítanie choreografie, pozri príklady a formát |
? | zobrazí načítanú choreografiu (kontrola po @) |
t | zatancuje načítanú choreografiu |
U | testuje ultrazvuk |
S | zobrazí aktuálnu polohu servomotorov (vhodné pri ladení choreografie) |
medzera | reset všetkých motorov do strednej polohy |
3,...,6 | zmiešaný pohyb (treba nastaviť vo funkciách kombinacia1() - kombinacia4()) |
v,b,n,j,h,g,f,s,l,o,i,u,y,r,e,w | zvukové efekty 1-16 |
TAB,*,/,+ | zahrá melódiu 1-4 |
W | zapísať choreografiu do EEPROM (opatrne) |
C | načítať choreografiu z EEPROM |
X | formátovať EEPROM na zápis choreografií (opatrne) |
V | vypni/zapni režim nudy |
A | automatický štart choreografie po zapnutí |
Pozrite si (a skopírujte si) príklady choreografií: Otto - príklady choreografií
Download: otto4.ino
1 #include <Servo.h>
2 #include <EEPROM.h>
3 #include <avr/pgmspace.h>
4
5 // odkomentujte nasledujuci riadok, ak je vas robot vytlaceny na 3D tlaciarni
6 //#define ROBOT_3D_PRINTED 1
7
8 #define ECHO_BT_TO_USB 1
9
10 #define US_TRIG 7
11 #define US_ECHO 8
12
13 #define BT_RX 2
14 #define BT_TX 4
15
16 #define SIRENA 12
17
18 //maximalna dlzka choreografie
19 #define CHOREO_LEN 200
20
21 #define CAS_OTTO_SA_ZACNE_NUDIT 9000
22
23 // cisla pinov, kde su zapojene servo motory
24 #define PIN_SERVO_LAVA_RUKA 10
25 #define PIN_SERVO_PRAVA_RUKA 11
26 #define PIN_SERVO_LAVA_NOHA 9
27 #define PIN_SERVO_PRAVA_NOHA 6
28 #define PIN_SERVO_LAVA_PATA 5
29 #define PIN_SERVO_PRAVA_PATA 3
30
31 // tu su serva cislovane 1-6
32 #define SERVO_LAVA_RUKA 5
33 #define SERVO_PRAVA_RUKA 6
34 #define SERVO_LAVA_NOHA 4
35 #define SERVO_PRAVA_NOHA 3
36 #define SERVO_LAVA_PATA 2
37 #define SERVO_PRAVA_PATA 1
38
39 // ak su niektore serva naopak, je tu jednotka
40 uint8_t servo_invertovane[6] = {0, 0, 0, 0, 0, 0};
41
42 // znaky, ktorymi sa ovladaju jednotlive stupne volnosti
43 char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' };
44 // co robia jednotlive znaky (znamienko urcuje smer)
45 int8_t zmeny[] = {-SERVO_LAVA_RUKA, SERVO_LAVA_RUKA,
46 SERVO_PRAVA_RUKA, -SERVO_PRAVA_RUKA,
47 -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA,
48 -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA,
49 SERVO_LAVA_PATA, -SERVO_LAVA_PATA,
50 -SERVO_PRAVA_PATA, SERVO_PRAVA_PATA
51 };
52
53 // sem si mozno ulozit svoju kalibraciu
54 uint8_t prednastavena_kalibracia[] = { 90, 90, 90, 90, 90, 90 };
55
56 uint8_t dolny_limit[] = {0, 0, 0, 0, 0, 0, 0, 0};
57 uint8_t horny_limit[] = {180, 180, 180, 180, 180, 180};
58
59 Servo s[6];
60
61 uint16_t ch_time[CHOREO_LEN];
62 uint8_t ch_servo[CHOREO_LEN];
63 uint8_t ch_val[CHOREO_LEN];
64 int ch_len;
65 uint8_t kalib[6];
66 int stav[6];
67 int krok;
68 uint8_t spomalenie;
69 uint8_t auto_start;
70
71 volatile int16_t distance;
72 uint32_t cas_ked_naposledy_stlacil;
73 uint8_t vypni_nudu;
74
75 void setup() {
76 Serial.begin(9600);
77 init_tone2();
78 init_serial(9600);
79 init_ultrasonic();
80
81 randomSeed(analogRead(1));
82 s[0].attach(PIN_SERVO_PRAVA_PATA);
83 s[1].attach(PIN_SERVO_LAVA_PATA);
84 s[2].attach(PIN_SERVO_PRAVA_NOHA);
85 s[3].attach(PIN_SERVO_LAVA_NOHA);
86 s[4].attach(PIN_SERVO_LAVA_RUKA);
87 s[5].attach(PIN_SERVO_PRAVA_RUKA);
88 precitaj_kalibraciu_z_EEPROM();
89 for (int i = 0; i < 6; i++)
90 {
91 kalib[i] = prednastavena_kalibracia[i];
92 stav[i] = kalib[i];
93 s[i].write(stav[i]);
94 }
95 ch_len = 0;
96 krok = 7;
97 vypni_nudu = 0;
98 spomalenie = 6;
99 ahoj();
100 ruky();
101 delay(100);
102 serial_println_flash(PSTR("\r\n Otto DTDT, verzia 2018/09/14"));
103 if (auto_start > 0)
104 {
105 delay(7000);
106 while (serial_available()) serial_read();
107 while (Serial.available()) Serial.read();
108 precitaj_choreografiu_z_EEPROM(auto_start);
109 zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
110 }
111 cas_ked_naposledy_stlacil = millis();
112 }
113
114 void loop() {
115 char z = -1;
116 if (serial_available()) z = serial_read();
117 #ifdef ECHO_BT_TO_USB
118 if (Serial.available()) z = Serial.read();
119 #endif
120
121 if (z != -1)
122 {
123 cas_ked_naposledy_stlacil = millis();
124 if (pohyb_znakom(z)) return;
125 else if (pohyb_kombinacia(z)) return;
126 else if (vydaj_zvukovy_efekt(z)) return;
127 else if (ma_zahrat_melodiu(z)) return;
128 else if (z == '@') nacitaj_choreografiu();
129 else if (z == '?') vypis_choreografiu();
130 else if (z == 't') zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
131 else if (z == '-') ahoj();
132 else if (z == ' ') reset();
133 else if (z == 'H') kalibruj();
134 else if (z == 'J') nastav_limity();
135 else if (z == 'G') vypis_kalibraciu();
136 else if (z == 'L') nacitaj_kalibraciu();
137 else if (z == 'E') zapis_kalibraciu_do_EEPROM();
138 else if (z == 'R') ruky();
139 else if (z == '9') zvys_krok();
140 else if (z == '1') zniz_krok();
141 else if (z == '8') zvys_spomalenie();
142 else if (z == '7') zniz_spomalenie();
143 else if (z == 'U') test_ultrazvuk();
144 else if (z == 'S') zobraz_stav();
145 else if (z == 'W') skus_zapisat_choreografiu_do_EEPROM();
146 else if (z == 'X') skus_naformatovat_EEPROM_choreografie();
147 else if (z == 'C') skus_nacitat_choreografiu_z_EEPROM();
148 else if (z == 'A') nastav_automaticky_start();
149 else if (z == 'V') prepni_nudu();
150 else if (z == '$') vasa_dalsia_funkcia();
151 }
152 int16_t d = measure_distance();
153 if (d < 10) menu_ultrasonic_request();
154
155 skus_ci_sa_nenudi();
156 }
157
158 void vasa_dalsia_funkcia()
159 {
160 serial_println_flash(PSTR("lalala"));
161 }
162
163 void nastav_automaticky_start()
164 {
165 serial_print_flash(PSTR("Automaticky start [1,2,3, 0=vypni]:"));
166 char z;
167 do { z = read_char(); } while ((z < '0') || (z > '3'));
168 if (z == '0') EEPROM.write(1, '~');
169 else EEPROM.write(1, z);
170 serial_println_char(z);
171 serial_println_flash(PSTR("ok"));
172 }
173
174 void prepni_nudu()
175 {
176 vypni_nudu ^= 1;
177 serial_print_flash(PSTR("ticho:"));
178 serial_println_num(vypni_nudu);
179 }
180
181 void skus_ci_sa_nenudi()
182 {
183 if ((millis() - cas_ked_naposledy_stlacil > CAS_OTTO_SA_ZACNE_NUDIT) && !vypni_nudu)
184 {
185 uint8_t rnd = random(21);
186 switch (rnd)
187 {
188 case 0: zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len); break;
189 case 1: zahraj_melodiu(1); break;
190 case 2: zahraj_melodiu(2); break;
191 case 3: zahraj_melodiu(3); break;
192 case 4: zahraj_melodiu(4); break;
193 case 5: for (int i = 0; i < 20; i++)
194 {
195 rnd = random(16) + 1;
196 zvukovy_efekt(rnd);
197 }
198 break;
199 default: zvukovy_efekt(rnd - 5); break;
200 }
201 cas_ked_naposledy_stlacil = millis();
202 }
203 }
204
205 const uint8_t klavesy_efektov[] PROGMEM = {'v', 'b', 'n', 'j', 'h', 'g', 'f', 's', 'l', 'o', 'i', 'u', 'y', 'r', 'e', 'w'};
206 #define POCET_KLAVES_EFEKTOV 16
207
208 uint8_t vydaj_zvukovy_efekt(char z)
209 {
210 for (int i = 0; i < POCET_KLAVES_EFEKTOV; i++)
211 if (z == pgm_read_byte(klavesy_efektov + i))
212 {
213 zvukovy_efekt(i + 1);
214 return 1;
215 }
216 return 0;
217 }
218
219 uint8_t ma_zahrat_melodiu(char z)
220 {
221 if (z == 9) zahraj_melodiu(1);
222 else if (z == '*') zahraj_melodiu(2);
223 else if (z == '/') zahraj_melodiu(3);
224 else if (z == '+') zahraj_melodiu(4);
225 else if (z == '!') zahraj_melodiu(5);
226 else if (z == 27) zastav_melodiu();
227 else return 0;
228 return 1;
229 }
230
231 void efekt1()
232 {
233 uint16_t fre = 100;
234 for (int i = 0; i < 200; i++)
235 {
236 tone2(fre, 2);
237 fre += 10;
238 delay(2);
239 }
240 }
241
242 void efekt2()
243 {
244 uint16_t fre = 2060;
245 for (int i = 0; i < 200; i++)
246 {
247 tone2(fre, 2);
248 fre -= 10;
249 delay(2);
250 }
251 }
252
253 void efekt3()
254 {
255 uint16_t fre;
256 for (int i = 0; i < 200; i++)
257 {
258 fre = random(3000) + 20;
259 tone2(fre, 2);
260 delay(2);
261 }
262 }
263
264 void efekt4()
265 {
266 uint16_t fre = 100;
267 for (int i = 0; i < 200; i++)
268 {
269 (i & 1) ? tone2(fre, 2) : tone2(2100 - fre, 2);
270 fre += 10;
271 delay(2);
272 }
273 }
274
275 void efekt5()
276 {
277 uint16_t fre = 100;
278 for (int i = 0; i < 100; i++)
279 {
280 (i & 1) ? tone2(fre, 4) : tone2(2100 - fre, 4);
281 fre += 20;
282 delay(4);
283 }
284 }
285
286 void efekt6()
287 {
288 uint16_t d = 12;
289 for (int i = 0; i < 20; i++)
290 {
291 tone2(1760, 10);
292 delay(d);
293 d += 3;
294 }
295 }
296
297 void efekt7()
298 {
299 for (int i = 0; i < 40; i++)
300 {
301 if (i & 1) tone2(1760, 10);
302 delay(10);
303 }
304 }
305
306 void efekt8()
307 {
308 uint16_t d = 72;
309 for (int i = 0; i < 20; i++)
310 {
311 tone2(1760, 10);
312 delay(d);
313 d -= 3;
314 }
315 }
316
317 void efekt9()
318 {
319 float fre = 3500;
320 while (fre > 30)
321 {
322 tone2((uint16_t)fre, 2);
323 fre *= 0.97;
324 delay(2);
325 }
326 }
327
328 void efekt10()
329 {
330 float fre = 30;
331 while (fre < 3000)
332 {
333 tone2((uint16_t)fre, 2);
334 fre *= 1.03;
335 delay(2);
336 }
337 }
338
339 void efekt11()
340 {
341 uint16_t fre = 3500;
342 uint16_t d = 42;
343 for (int i = 0; i < 20; i++)
344 {
345 tone2(fre, d);
346 fre -= 165;
347 delay(d);
348 d -= 2;
349 }
350 }
351
352 void efekt12()
353 {
354 float fre = 110;
355 uint8_t d = 42;
356 for (int i = 0; i < 20; i++)
357 {
358 tone2(fre, d);
359 fre += 165;
360 delay(d);
361 d -= 2;
362 }
363 }
364
365 void efekt13()
366 {
367 uint16_t fre = 3400;
368 uint8_t d = 200;
369 uint8_t delta = 1;
370 for (int i = 0; i < 20; i++)
371 {
372 tone2(fre, d);
373 fre -= 165;
374 delay(d);
375 d -= delta;
376 delta++;
377 }
378 tone2(110, 1000);
379 delay(1000);
380 }
381
382 void efekt14()
383 {
384 uint16_t fre = 880;
385 int16_t d = 20;
386 for (int i = 0; i < 20; i++)
387 {
388 tone2(fre, d);
389 delay(d);
390 fre += random(50) - 25;
391 d += random(10) - 5;
392 if (d < 0) d = 1;
393 }
394 }
395
396 void efekt15()
397 {
398 uint16_t fre = 440;
399 int16_t d = 20;
400 for (int i = 0; i < 20; i++)
401 {
402 tone2(fre, d);
403 delay(d);
404 fre += random(25) - 12;
405 d += random(10) - 5;
406 if (d < 0) d = 1;
407 }
408 }
409
410 void efekt16()
411 {
412 uint16_t fre = 1760;
413 int16_t d = 20;
414 for (int i = 0; i < 20; i++)
415 {
416 tone2(fre, d);
417 delay(d);
418 fre += random(100) - 50;
419 d += random(10) - 5;
420 if (d < 0) d = 1;
421 }
422 }
423
424 void zvukovy_efekt(uint8_t cislo)
425 {
426 switch (cislo) {
427 case 1: efekt1(); break;
428 case 2: efekt2(); break;
429 case 3: efekt3(); break;
430 case 4: efekt4(); break;
431 case 5: efekt5(); break;
432 case 6: efekt6(); break;
433 case 7: efekt7(); break;
434 case 8: efekt8(); break;
435 case 9: efekt9(); break;
436 case 10: efekt10(); break;
437 case 11: efekt11(); break;
438 case 12: efekt12(); break;
439 case 13: efekt13(); break;
440 case 14: efekt14(); break;
441 case 15: efekt15(); break;
442 case 16: efekt16(); break;
443 }
444 }
445
446 void test_ultrazvuk()
447 {
448 while ((serial_available() == 0) && (Serial.available() == 0))
449 {
450 serial_println_num(measure_distance());
451 delay(100);
452 }
453 serial_read();
454 }
455
456 void menu_ultrasonic_request()
457 {
458 uint32_t tm = millis();
459 int d = measure_distance();
460 int count = 0;
461 int count2 = 0;
462 while ((millis() - tm < 1500) && ((d < 15) || (count2 < 5)) && (count < 10))
463 {
464 delay(10);
465 d = measure_distance();
466 if (d == 10000) count++;
467 else count = 0;
468 if (d >= 15) count2++;
469 else count2 = 0;
470 if (serial_available() || Serial.available()) return;
471 }
472 if (millis() - tm >= 1500)
473 {
474 ultrasonic_menu();
475 cas_ked_naposledy_stlacil = millis();
476 }
477 }
478
479 void ultrasonic_menu()
480 {
481 int selection = 0;
482 tone2( 880, 200);
483
484 do {
485 int count = 0;
486 int cnt10000 = 0;
487 do {
488 int32_t d = measure_distance();
489 if (d == 10000)
490 {
491 cnt10000++;
492 delay(10);
493 if (cnt10000 == 30) break;
494 continue;
495 }
496 if (d >= 20) count++;
497 else count = 0;
498 delay(10);
499 } while (!serial_available() && !Serial.available() && (count < 20));
500
501 tone2( 440, 200);
502 uint32_t tm = millis();
503 while ((millis() - tm < 2000) && !serial_available() && !Serial.available())
504 {
505 int32_t d = measure_distance();
506 if (d < 15) count++;
507 else count = 0;
508 if (count > 10) break;
509 delay(20);
510 }
511
512 if (millis() - tm >= 2000)
513 {
514 tone2( 2000, 50);
515 menu_command(selection);
516 return;
517 }
518 selection++;
519 for (int i = 0; i < selection; i++)
520 {
521 tone2( 1261, 50);
522 delay(250);
523 }
524 } while (!serial_available() && !Serial.available());
525 while (serial_available()) serial_read();
526 while (Serial.available()) Serial.read();
527 }
528
529 void menu_command(int cmd)
530 {
531 if (cmd == 1) vpred();
532 if (cmd == 2) {
533 precitaj_choreografiu_z_EEPROM(1);
534 zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
535 }
536 if (cmd == 3) {
537 precitaj_choreografiu_z_EEPROM(2);
538 zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
539 }
540 if (cmd == 4) {
541 precitaj_choreografiu_z_EEPROM(3);
542 zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
543 }
544 if (cmd == 5) zahraj_melodiu(1);
545 if (cmd == 6) melodia_jedna_druhej();
546 if (cmd == 7) ahoj();
547 if (cmd == 8) zahraj_melodiu(2);
548 if (cmd == 9) zahraj_melodiu(3);
549 if (cmd == 10) zahraj_melodiu(4);
550 if (cmd == 11) zahraj_melodiu(5);
551 }
552
553 void melodia_jedna_druhej()
554 {
555 for (int i = 0; i < 2; i++)
556 {
557 tone2(262, 200);
558 delay(200);
559 tone2(330, 200);
560 delay(200);
561
562 tone2(262, 200);
563 delay(200);
564 tone2(330, 200);
565 delay(200);
566
567 tone2(392, 400);
568 delay(400);
569
570 tone2(392, 400);
571 delay(400);
572 }
573 }
574
575 // chodza pre vacsieho dreveneho robota
576 uint16_t chor_time[] = {500, 100, 1, 1, 100, 1, 1, 1, 100, 1, 1, 1, 100, 1, 100, 1, 1, 0};
577 uint8_t chor_servo[] = {11, 1, 2, 6, 4, 3, 6, 5, 1, 2, 6, 5, 3, 4, 1, 2, 9, 0};
578 uint8_t chor_val[] = {1, 48, 69, 180, 104, 104, 90, 0, 111, 146, 0, 90, 62, 69, 48, 69, 2, 0};
579 uint8_t chor_len = 18;
580
581 uint16_t fwd_time[] = {1,1,100,1,1,1,100,1,1,1,100,1,100,1,1,100,1,100,1,1,1,0};
582 uint8_t fwd_servo[] = {10,11,1,2,1,6,4,3,6,5,1,2,2,6,5,3,4,1,2,1,9,0};
583 uint8_t fwd_val[] = {6,1,36,66,90,180,120,120,90,0,109,156,90,0,90,60,60,36,70,90,6,0};
584 uint8_t fwd_len = 22;
585
586 uint8_t bwd_val[] = {6,2,36,66,90,180,60,60,90,0,109,156,90,0,90,120,120,36,70,90,6,0};
587 uint8_t lt_val[] = {6,5,36,66,90,180,120,80,90,0,111,156,90,0,90,110,80,36,70,90,6,0};
588 uint8_t rt_val[] = {6,4,36,66,90,180,80,120,90,0,109,156,90,0,90,80,110,36,70,90,6,0};
589
590
591 void vpred()
592 {
593 while ((measure_distance() > 10) && !serial_available() && !Serial.available())
594 zatancuj_choreografiu(chor_time, chor_servo, chor_val, chor_len);
595 pipni();
596 reset();
597 }
598
599 void chod_smerom(int s)
600 {
601 if (s == 1) zatancuj_choreografiu(fwd_time, fwd_servo, fwd_val, fwd_len);
602 else if (s == 2) zatancuj_choreografiu(fwd_time, fwd_servo, bwd_val, fwd_len);
603 else if (s == 3) zatancuj_choreografiu(fwd_time, fwd_servo, rt_val, fwd_len);
604 else if (s == 4) zatancuj_choreografiu(fwd_time, fwd_servo, lt_val, fwd_len);
605 }
606 void pipni()
607 {
608 tone2( 1568, 50);
609 delay(100);
610 tone2( 1357, 50);
611 }
612
613 void ruky()
614 {
615 uint8_t odloz_spomalenie = spomalenie;
616 spomalenie = 0;
617 #ifdef ROBOT_3D_PRINTED
618 nastav_koncatinu(5, 180);
619 nastav_koncatinu(6, 180);
620 #else
621 nastav_koncatinu(5, 0);
622 nastav_koncatinu(6, 0);
623 #endif
624 delay(1000);
625 nastav_koncatinu(5, 90);
626 nastav_koncatinu(6, 90);
627 spomalenie = odloz_spomalenie;
628 delay(1000);
629 pipni();
630 }
631
632 void ahoj()
633 {
634 tone2( 1568, 50);
635 delay(70);
636 tone2( 1175, 30);
637 delay(50);
638 tone2( 880, 30);
639 delay(50);
640 tone2( 1047, 50);
641 delay(70);
642 tone2( 1245, 30);
643 delay(150);
644 tone2( 1568, 50);
645 delay(100);
646 if (random(10) > 4) tone2( 1357, 50);
647 else tone2( 1047, 50);
648 }
649
650 void nastav_koncatinu(int8_t srv, uint8_t poloha)
651 {
652 int8_t delta;
653 srv--;
654
655 if (servo_invertovane[srv]) poloha = 180 - poloha;
656
657 if ((int16_t)poloha + (int16_t)kalib[srv] - 90 < 0) poloha = 0;
658 else poloha += kalib[srv] - 90;
659
660 if (poloha > 180) poloha = 180;
661
662 if (spomalenie == 0) {
663 stav[srv] = poloha;
664 s[srv].write(stav[srv]);
665 }
666 else {
667 if (stav[srv] < poloha) delta = 1;
668 else delta = -1;
669 while (stav[srv] != poloha)
670 {
671 stav[srv] += delta;
672 s[srv].write(stav[srv]);
673 delay(spomalenie);
674 }
675 }
676 }
677
678 void zobraz_stav()
679 {
680 for (int i = 0; i < 6; i++)
681 {
682 serial_print_flash(PSTR("S")); serial_print_num(i + 1); serial_print_flash(PSTR(": ")); serial_println_num(stav[i] - kalib[i] + 90);
683 }
684 serial_println_flash(PSTR("---"));
685 pipni();
686 }
687
688 void pohyb(int8_t servo)
689 {
690 int8_t srv = (servo > 0) ? servo : -servo;
691 srv--;
692 if (servo_invertovane[srv]) servo = -servo;
693 if (servo > 0)
694 {
695 if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
696 else stav[srv] = horny_limit[srv];
697 s[srv].write(stav[srv]);
698 }
699 else if (servo < 0)
700 {
701 if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok;
702 else stav[srv] = dolny_limit[srv];
703 s[srv].write(stav[srv]);
704 }
705 }
706
707 uint8_t pohyb_znakom(char z)
708 {
709 for (int i = 0; i < 12; i++)
710 {
711 if (z == znaky_zmien[i])
712 {
713 int8_t servo = zmeny[i];
714 pohyb(servo);
715 return 1;
716 }
717 }
718 return 0;
719 }
720
721 void kombinacia1()
722 {
723 chod_smerom(1);
724 // pohyb(SERVO_LAVA_NOHA);
725 // pohyb(-SERVO_PRAVA_PATA);
726 }
727
728 void kombinacia2()
729 {
730 chod_smerom(2);
731
732 // pohyb(SERVO_PRAVA_NOHA);
733 // pohyb(-SERVO_LAVA_PATA);
734 }
735
736 void kombinacia3()
737 {
738 chod_smerom(3);
739
740 // pohyb(SERVO_LAVA_RUKA);
741 // pohyb(SERVO_PRAVA_RUKA);
742 }
743
744 void kombinacia4()
745 {
746 chod_smerom(4);
747
748 // pohyb(-SERVO_LAVA_RUKA);
749 // pohyb(-SERVO_PRAVA_RUKA);
750 }
751
752 int pohyb_kombinacia(char z)
753 {
754 if (z == '3') kombinacia1();
755 else if (z == '4') kombinacia2();
756 else if (z == '5') kombinacia3();
757 else if (z == '6') kombinacia4();
758 else return 0;
759 return 1;
760 }
761
762 char read_char()
763 {
764 while (!serial_available() && !Serial.available());
765 if (serial_available()) return serial_read();
766 else return Serial.read();
767 }
768
769 int nacitajCislo()
770 {
771 int num = 0;
772 int z;
773 do {
774 z = read_char();
775 if (z == '#') while (z != 13) z = read_char();
776 } while ((z < '0') || (z > '9'));
777 while ((z >= '0') && (z <= '9'))
778 {
779 num *= 10;
780 num += (z - '0');
781 do {
782 z = read_char();
783 if (z == -1) delayMicroseconds(10);
784 } while (z < 0);
785 }
786 return num;
787 }
788
789 void nacitaj_choreografiu()
790 {
791 ch_len = 0;
792 int tm;
793 do {
794 tm = nacitajCislo();
795 ch_time[ch_len] = tm;
796 ch_servo[ch_len] = nacitajCislo();
797 ch_val[ch_len] = nacitajCislo();
798 ch_len++;
799 if (ch_len == CHOREO_LEN) break;
800 } while (tm > 0);
801 pipni();
802 }
803
804 void vypis_choreografiu()
805 {
806 for (int i = 0; i < ch_len; i++)
807 {
808 serial_print_num(ch_time[i]);
809 serial_print(" ");
810 serial_print_num(ch_servo[i]);
811 serial_print(" ");
812 serial_println_num(ch_val[i]);
813 }
814 serial_println_flash(PSTR("---"));
815 pipni();
816 }
817
818 uint32_t koniec_tanca;
819
820 void zatancuj_choreografiu(uint16_t *ch_time, uint8_t *ch_servo, uint8_t *ch_val, int ch_len )
821 {
822 koniec_tanca = millis() + 3600000;
823 for (int i = 0; i < ch_len - 1; i++)
824 {
825 delay(ch_time[i]);
826 if (millis() > koniec_tanca) break;
827 if (ch_servo[i] < 7) nastav_koncatinu(ch_servo[i], ch_val[i]);
828 else i = specialny_prikaz(i, ch_servo[i], ch_val[i], ch_len);
829 if (serial_available() || Serial.available()) break;
830 }
831 pipni();
832 }
833
834 int specialny_prikaz(uint16_t i, uint8_t prikaz, uint8_t argument, int len)
835 {
836 if (prikaz == 8) koniec_tanca = millis() + 1000 * (uint32_t)argument;
837 else if (prikaz == 9)
838 {
839 if (serial_available() || Serial.available()) return len - 1;
840 if (measure_distance() < 10) return len - 1;
841 return ((int)argument) - 1;
842 }
843 else if (prikaz == 10) spomalenie = argument;
844 else if (prikaz == 11) zahraj_melodiu(argument);
845 else if (prikaz == 12) zvukovy_efekt(argument);
846 return i;
847 }
848
849 void reset()
850 {
851 for (int i = 0; i < 6; i++)
852 {
853 stav[i] = kalib[i];
854 s[i].write(kalib[i]);
855 delay(300);
856 }
857 pipni();
858 }
859
860 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota, uint8_t min_hodnota, uint8_t max_hodnota)
861 {
862 serial_print_flash(PSTR(" (+/-/ENTER): "));
863 serial_println_num(hodnota);
864 s[servo].write(hodnota);
865 char z;
866 do {
867 z = read_char();
868 if ((z == '+') && (hodnota < max_hodnota)) hodnota++;
869 else if ((z == '-') && (hodnota > min_hodnota)) hodnota--;
870 if ((z == '+') || (z == '-'))
871 {
872 serial_print_num(hodnota); serial_print_char('\r');
873 s[servo].write(hodnota);
874 }
875 } while (z != 13);
876 return hodnota;
877 }
878
879 void kalibruj()
880 {
881 for (int i = 0; i < 6; i++)
882 {
883 serial_print_num(i);
884 kalib[i] = nalad_hodnotu_serva(i, kalib[i], 90-63, 90+63);
885 serial_print_num(i);
886 serial_print(": ");
887 serial_println_num(kalib[i]);
888 }
889 for (int i = 0; i < 6; i++) {
890 serial_print_num(kalib[i]);
891 serial_print(" ");
892 }
893 serial_print_flash(PSTR(" Chyt robota zozadu za telo (ENTER):"));
894 char z;
895 do { z = read_char(); } while (z != 13);
896 serial_println_char(z);
897
898 serial_print_flash(PSTR("*** Nasledujuce koncatiny su lave/prave z pohladu robota.\r\nAk to nesedi, treba prehodit kabliky servo!\r\n"));
899
900 for (int i = 0; i < 6; i++) servo_invertovane[i] = 0;
901
902 nastav_koncatinu(SERVO_PRAVA_PATA, 0);
903 serial_print_flash(PSTR("Prava pata: je von alebo dnu? (V/D):"));
904 do { z = read_char(); } while ((z != 'V') && (z != 'D'));
905 serial_println_char(z);
906 nastav_koncatinu(SERVO_PRAVA_PATA, 90);
907 if (z == 'D') servo_invertovane[SERVO_PRAVA_PATA - 1] = 0;
908 else servo_invertovane[SERVO_PRAVA_PATA - 1] = 1;
909
910 nastav_koncatinu(SERVO_LAVA_PATA, 0);
911 serial_print_flash(PSTR("Lava pata: je von alebo dnu? (V/D):"));
912 do { z = read_char(); } while ((z != 'V') && (z != 'D'));
913 serial_println_char(z);
914 nastav_koncatinu(SERVO_LAVA_PATA, 90);
915 if (z == 'V') servo_invertovane[SERVO_LAVA_PATA - 1] = 0;
916 else servo_invertovane[SERVO_LAVA_PATA - 1] = 1;
917
918 nastav_koncatinu(SERVO_PRAVA_NOHA, 0);
919 serial_print_flash(PSTR("Prava noha: vpredu je pata alebo spicka? (P/S):"));
920 do { z = read_char(); } while ((z != 'P') && (z != 'S'));
921 serial_println_char(z);
922 nastav_koncatinu(SERVO_PRAVA_NOHA, 90);
923 if (z == 'P') servo_invertovane[SERVO_PRAVA_NOHA - 1] = 1;
924 else servo_invertovane[SERVO_PRAVA_NOHA - 1] = 0;
925
926 nastav_koncatinu(SERVO_LAVA_NOHA, 0);
927 serial_print_flash(PSTR("Lava noha: vpredu je pata alebo spicka? (P/S):"));
928 do { z = read_char(); } while ((z != 'P') && (z != 'S'));
929 serial_println_char(z);
930 nastav_koncatinu(SERVO_LAVA_NOHA, 90);
931 if (z == 'S') servo_invertovane[SERVO_LAVA_NOHA - 1] = 1;
932 else servo_invertovane[SERVO_LAVA_NOHA - 1] = 0;
933
934 nastav_koncatinu(SERVO_PRAVA_RUKA, 0);
935 serial_print_flash(PSTR("Prava ruka: je hore alebo dole? (H/D):"));
936 do { z = read_char(); } while ((z != 'H') && (z != 'D'));
937 serial_println_char(z);
938 nastav_koncatinu(SERVO_PRAVA_RUKA, 90);
939 if (z == 'H') servo_invertovane[SERVO_PRAVA_RUKA - 1] = 1;
940 else servo_invertovane[SERVO_PRAVA_RUKA - 1] = 0;
941
942 nastav_koncatinu(SERVO_LAVA_RUKA, 0);
943 serial_print_flash(PSTR("Lava ruka: je hore alebo dole? (H/D):"));
944 do { z = read_char(); } while ((z != 'H') && (z != 'D'));
945 serial_println_char(z);
946 nastav_koncatinu(SERVO_LAVA_RUKA, 90);
947 if (z == 'D') servo_invertovane[SERVO_PRAVA_RUKA - 1] = 0;
948 else servo_invertovane[SERVO_PRAVA_RUKA - 1] = 1;
949
950 serial_println_flash(PSTR("ok"));
951 pipni();
952 }
953
954 void nastav_limity()
955 {
956 for (int i = 0; i < 6; i++)
957 {
958 serial_print_num(i);
959 serial_print_flash(PSTR("dolny"));
960 dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i], 0, 180);
961 serial_print_num(i);
962 serial_print_flash(PSTR(" dolny: "));
963 serial_println_num(dolny_limit[i]);
964 s[i].write(kalib[i]);
965
966 serial_print_num(i);
967 serial_print_flash(PSTR("horny"));
968 horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i], 0, 180);
969 serial_print_num(i);
970 serial_print_flash(PSTR(" horny: "));
971 serial_println_num(horny_limit[i]);
972 s[i].write(kalib[i]);
973 }
974 for (int i = 0; i < 6; i++) {
975 serial_print_num(dolny_limit[i]);
976 serial_print("-");
977 serial_print_num(horny_limit[i]);
978 serial_print(" ");
979 }
980 serial_println_flash(PSTR("ok"));
981 pipni();
982 }
983
984 void vypis_kalibraciu()
985 {
986 serial_print_flash(PSTR("stredy: "));
987 for (int i = 0; i < 6; i++) {
988 serial_print_num(kalib[i]);
989 serial_print(" ");
990 }
991 serial_println();
992
993 serial_print_flash(PSTR("invertovane servo: "));
994 for (int i = 0; i < 6; i++)
995 {
996 serial_print_num(servo_invertovane[i]);
997 serial_print_char(' ');
998 }
999 serial_println();
1000
1001 serial_print_flash(PSTR("dolny limit: "));
1002 for (int i = 0; i < 6; i++) {
1003 serial_print_num(dolny_limit[i]);
1004 serial_print(" ");
1005 }
1006 serial_println();
1007 serial_print_flash(PSTR("horny limit: "));
1008 for (int i = 0; i < 6; i++) {
1009 serial_print_num(horny_limit[i]);
1010 serial_print(" ");
1011 }
1012 serial_println();
1013 }
1014
1015 void nacitaj_kalibraciu()
1016 {
1017 for (int i = 0; i < 6; i++)
1018 kalib[i] = nacitajCislo();
1019 vypis_kalibraciu();
1020 serial_println_flash(PSTR("ok"));
1021 pipni();
1022 }
1023
1024 void zvys_krok()
1025 {
1026 if (krok < 180) krok++;
1027 serial_print_flash(PSTR("krok: "));
1028 serial_println_num(krok);
1029 }
1030
1031 void zniz_krok()
1032 {
1033 if (krok > 0) krok--;
1034 serial_print_flash(PSTR("krok: "));
1035 serial_println_num(krok);
1036 }
1037
1038 void zniz_spomalenie()
1039 {
1040 if (spomalenie > 0) spomalenie--;
1041 serial_print_flash(PSTR("spomalenie: "));
1042 serial_println_num(spomalenie);
1043 }
1044
1045 void zvys_spomalenie()
1046 {
1047 if (spomalenie < 100) spomalenie++;
1048 serial_print_flash(PSTR("spomalenie: "));
1049 serial_println_num(spomalenie);
1050 }
1051
1052 //nasleduju funkcie ktore pracuju s pamatou EEPROM
1053
1054 //EEPROM MAP:
1055 // 0: slot number where choreography 3 starts (B3. Real address = B3 x 4)
1056 // 1: marker '~' indicates calibration is already stored in EEPROM and will be loaded on startup
1057 // '1', '2', '3' indicate the same, but choreography 1,2, or 3 is automatically launched on startup
1058 // 2-8: servo calibration (central points to be used instead of default 90)
1059 // they are shifted by (90-63) and the range is (90-63)...0 till (90+63)...127
1060 // bit 7 (value 128) indicates servo is inverted
1061 // 9-14: lower limit for direct control for all 6 servos
1062 // 15-20: upper limit for direct control for all 6 servos
1063 // 21: number of steps in choreography 1 (L1) 0=choreography not in memory
1064 // 22: number of steps in choreography 2 (L2) 0=not in memory
1065 // 23: number of steps in choreography 3 (L3) 0=not in memory
1066 // 24..(L1 x 4 + 23) choreography1 tuplets (uint16,uint8,uint8) x L1
1067 // (1024 - L2 x 4)..1023 choreography2 same as above
1068 // B3 x 4..(B3 x 4 + L3 x 4 - 1) choreography 3 same as above
1069
1070 void skus_zapisat_choreografiu_do_EEPROM()
1071 {
1072 serial_print_flash(PSTR("Cislo? [1-3]: "));
1073 char odpoved = read_char();
1074 if ((odpoved < '1') || (odpoved > '3')) return;
1075 serial_println_char(odpoved);
1076 uint8_t cislo = odpoved - '0';
1077
1078 serial_print_flash(PSTR("Zapisat choreografiu do EEPROM c."));
1079 serial_print_char(odpoved);
1080 serial_print_flash(PSTR("? [Y/n]: "));
1081 odpoved = read_char();
1082 serial_println_char(odpoved);
1083 if (odpoved == 'Y')
1084 zapis_choreografiu_do_EEPROM(cislo);
1085 }
1086
1087 void skus_nacitat_choreografiu_z_EEPROM()
1088 {
1089 serial_print_flash(PSTR("Cislo? [1-3]: "));
1090 char odpoved = read_char();
1091 if ((odpoved < '1') || (odpoved > '3')) return;
1092 serial_println_char(odpoved);
1093 uint8_t cislo = odpoved - '0';
1094
1095 serial_print_flash(PSTR("Precitat choreografiu z EEPROM c."));
1096 serial_print_char(odpoved);
1097 serial_print_flash(PSTR("? [Y/n]: "));
1098 odpoved = read_char();
1099 serial_println_char(odpoved);
1100 if (odpoved == 'Y')
1101 precitaj_choreografiu_z_EEPROM(cislo);
1102 }
1103
1104 void skus_naformatovat_EEPROM_choreografie()
1105 {
1106 serial_print_flash(PSTR("Formatovat EEPROM choreografii? [Y/n]:"));
1107 char odpoved = read_char();
1108 serial_println_char(odpoved);
1109 if (odpoved == 'Y')
1110 naformatuj_EEPROM_choreografie();
1111 }
1112
1113 void zapis_choreografiu_do_EEPROM(int slot)
1114 {
1115 uint8_t b3 = EEPROM.read(0);
1116 uint8_t len1 = EEPROM.read(21);
1117 uint8_t len2 = EEPROM.read(22);
1118 uint8_t len3 = EEPROM.read(23);
1119 uint16_t wp = 65535;
1120
1121 if ((len1 > CHOREO_LEN) || (len2 > CHOREO_LEN) || (len3 > CHOREO_LEN) || (b3 > 250 - len3) || ((len1 > b3) && (b3 != 0)) || (len3 + b3 > 250 - len2) || (len1 + len2 > 250))
1122 b3 = len1 = len2 = len3 = 0;
1123
1124 if (slot == 1)
1125 {
1126 if (((ch_len < b3) || (len3 == 0)) && (ch_len + len2 + len3 <= 250))
1127 {
1128 EEPROM.write(21, ch_len);
1129 wp = 24;
1130 }
1131 }
1132 else if (slot == 2)
1133 {
1134 if ((250 - b3 - len3 > ch_len) && (ch_len + len1 + len3 <= 250))
1135 {
1136 EEPROM.write(22, ch_len);
1137 wp = 1000 - ch_len * 4;
1138 }
1139 }
1140 else if (slot == 3)
1141 {
1142 if (ch_len + len1 + len2 <= 250)
1143 {
1144 EEPROM.write(23, ch_len);
1145 EEPROM.write(0, (len1 - len2) / 2 + 125);
1146 wp = 4 * ((len1 - len2) / 2 + 125);
1147 }
1148 }
1149
1150 if (wp == 65535)
1151 serial_println_flash(PSTR("not enough space"));
1152 else
1153 {
1154 for (int i = 0; i < ch_len; i++)
1155 {
1156 EEPROM.write(wp + 4 * i, ch_time[i] & 255);
1157 EEPROM.write(wp + 1 + 4 * i, ch_time[i] >> 8);
1158 EEPROM.write(wp + 2 + 4 * i, ch_servo[i]);
1159 EEPROM.write(wp + 3 + 4 * i, ch_val[i]);
1160 }
1161 serial_println_flash(PSTR("ok"));
1162 }
1163 }
1164
1165 void precitaj_choreografiu_z_EEPROM(uint8_t slot)
1166 {
1167 uint8_t b3 = EEPROM.read(0);
1168 uint8_t len1 = EEPROM.read(21);
1169 uint8_t len2 = EEPROM.read(22);
1170 uint8_t len3 = EEPROM.read(23);
1171 uint16_t rp = 65535;
1172 if ((len1 > CHOREO_LEN) || (len2 > CHOREO_LEN) || (len3 > CHOREO_LEN) || (b3 > 250 - len3) || ((len1 > b3) && (b3 != 0)) || (len3 + b3 > 250 - len2) || (len1 + len2 > 250))
1173 b3 = len1 = len2 = len3 = 0;
1174
1175 if (slot == 1)
1176 {
1177 if (len1 > 0)
1178 {
1179 rp = 24;
1180 ch_len = len1;
1181 }
1182 }
1183 else if (slot == 2)
1184 {
1185 if (len2 > 0)
1186 {
1187 ch_len = len2;
1188 rp = 1000 - ch_len * 4;
1189 }
1190 }
1191 else if (slot == 3)
1192 {
1193 if (len3 > 0)
1194 {
1195 rp = b3 * 4;
1196 ch_len = len3;
1197 }
1198 }
1199
1200 if (rp == 65535)
1201 serial_println_flash(PSTR("couldn't"));
1202 else
1203 {
1204 for (int i = 0; i < ch_len; i++)
1205 {
1206 ch_time[i] = ((uint16_t)EEPROM.read(rp + 4 * i)) |
1207 (((uint16_t)EEPROM.read(rp + 1 + 4 * i)) << 8);
1208 ch_servo[i] = EEPROM.read(rp + 2 + 4 * i);
1209 ch_val[i] = EEPROM.read(rp + 3 + 4 * i);
1210 }
1211 serial_println_flash(PSTR("ok"));
1212 pipni();
1213 }
1214 }
1215
1216 void naformatuj_EEPROM_choreografie()
1217 {
1218 EEPROM.write(0, 0);
1219 EEPROM.write(21, 0);
1220 EEPROM.write(22, 0);
1221 EEPROM.write(23, 0);
1222 serial_println_flash(PSTR("ok"));
1223 pipni();
1224 }
1225
1226 void precitaj_kalibraciu_z_EEPROM()
1227 {
1228 uint8_t value = EEPROM.read(1);
1229 if ((value != '~') && (value != '1') && (value != '2') && (value != '3')) return;
1230 if (value != '~') auto_start = value - '0';
1231 else auto_start = 0;
1232 for (int i = 2; i < 8; i++)
1233 {
1234 int16_t k = EEPROM.read(i);
1235 if (k > 127) servo_invertovane[i - 2] = 1;
1236 else servo_invertovane[i - 2] = 0;
1237 k &= 127;
1238 prednastavena_kalibracia[i - 2] = k + (90-63);
1239 }
1240 for (int i = 0; i < 6; i++)
1241 dolny_limit[i] = EEPROM.read(i + 9);
1242 for (int i = 0; i < 6; i++)
1243 horny_limit[i] = EEPROM.read(i + 15);
1244 }
1245
1246 void zapis_kalibraciu_do_EEPROM()
1247 {
1248 serial_print_flash(PSTR("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: "));
1249 char odpoved = read_char();
1250 serial_println_char(odpoved);
1251 if (odpoved == 'Y')
1252 {
1253 char kalib_state = EEPROM.read(1);
1254 if ((kalib_state == '~') || ((kalib_state >= '1') && (kalib_state <= '3')))
1255 EEPROM.write(1, kalib_state);
1256 else EEPROM.write(1, '~');
1257 for (int i = 2; i < 8; i++)
1258 {
1259 int16_t k = kalib[i - 2] - (90-63);
1260 if (k < 0) k = 0;
1261 else if (k > 127) k = 127;
1262 if (servo_invertovane[i - 2]) k += 128;
1263 EEPROM.write(i, (uint8_t)k);
1264 }
1265 for (int i = 0; i < 6; i++)
1266 EEPROM.write(9 + i, dolny_limit[i]);
1267 for (int i = 0; i < 6; i++)
1268 EEPROM.write(15 + i, horny_limit[i]);
1269 serial_println_flash(PSTR("ok"));
1270 }
1271 }
1272
1273 // nasleduje softverova implementacia serioveho portu
1274 #define SERIAL_STATE_IDLE 0
1275 #define SERIAL_STATE_RECEIVING 1
1276 #define SERIAL_BUFFER_LENGTH 20
1277
1278 static volatile uint8_t serial_state;
1279 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
1280 static volatile uint8_t serial_buf_wp, serial_buf_rp;
1281
1282 static volatile uint8_t receiving_byte;
1283
1284 static volatile uint32_t time_startbit_noticed;
1285 static volatile uint8_t next_bit_order;
1286 static volatile uint8_t waiting_stop_bit;
1287 static uint16_t one_byte_duration;
1288 static uint16_t one_bit_duration;
1289 static uint16_t one_bit_write_duration;
1290 static uint16_t half_of_one_bit_duration;
1291
1292 void init_serial(uint32_t baud_rate)
1293 {
1294 pinMode(2, INPUT);
1295 pinMode(4, OUTPUT);
1296
1297 serial_state = SERIAL_STATE_IDLE;
1298
1299 one_byte_duration = 9500000 / baud_rate;
1300 one_bit_duration = 1000000 / baud_rate;
1301 one_bit_write_duration = one_bit_duration - 1;
1302 half_of_one_bit_duration = 500000 / baud_rate;
1303
1304 PCMSK2 |= 4; //PCINT18;
1305 PCIFR &= ~4; //PCIF2;
1306 PCICR |= 4; // PCIE2;
1307 }
1308
1309 ISR(PCINT2_vect)
1310 {
1311 uint32_t tm = micros();
1312 if (serial_state == SERIAL_STATE_IDLE)
1313 {
1314 time_startbit_noticed = tm;
1315 serial_state = SERIAL_STATE_RECEIVING;
1316 receiving_byte = 0xFF;
1317 next_bit_order = 0;
1318 }
1319 else if (tm - time_startbit_noticed > one_byte_duration)
1320 {
1321 serial_buffer[serial_buf_wp] = receiving_byte;
1322 serial_buf_wp++;
1323 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1324 time_startbit_noticed = tm;
1325 receiving_byte = 0xFF;
1326 next_bit_order = 0;
1327 }
1328 else if (PIND & 4)
1329 {
1330 int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
1331 while (next_bit_order < new_next_bit_order)
1332 {
1333 receiving_byte &= ~(1 << next_bit_order);
1334 next_bit_order++;
1335 }
1336 if (next_bit_order == 8)
1337 {
1338 serial_buffer[serial_buf_wp] = receiving_byte;
1339 serial_buf_wp++;
1340 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1341 serial_state = SERIAL_STATE_IDLE;
1342 }
1343 } else
1344 next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
1345 }
1346
1347 uint8_t serial_available()
1348 {
1349 cli();
1350 if (serial_buf_rp != serial_buf_wp)
1351 {
1352 sei();
1353 return 1;
1354 }
1355 if (serial_state == SERIAL_STATE_RECEIVING)
1356 {
1357 uint32_t tm = micros();
1358 if (tm - time_startbit_noticed > one_byte_duration)
1359 {
1360 serial_state = SERIAL_STATE_IDLE;
1361 serial_buffer[serial_buf_wp] = receiving_byte;
1362 serial_buf_wp++;
1363 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1364 sei();
1365 return 1;
1366 }
1367 }
1368 sei();
1369 return 0;
1370 }
1371
1372 int16_t serial_read()
1373 {
1374 cli();
1375 if (serial_buf_rp != serial_buf_wp)
1376 {
1377 uint8_t ch = serial_buffer[serial_buf_rp];
1378 serial_buf_rp++;
1379 if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
1380 sei();
1381 return ch;
1382 }
1383
1384 if (serial_state == SERIAL_STATE_RECEIVING)
1385 {
1386 uint32_t tm = micros();
1387 if (tm - time_startbit_noticed > one_byte_duration)
1388 {
1389 uint8_t ch = receiving_byte;
1390 serial_state = SERIAL_STATE_IDLE;
1391 sei();
1392 return ch;
1393 }
1394 }
1395 sei();
1396 return -1;
1397 }
1398
1399 void serial_write(uint8_t ch)
1400 {
1401 #ifdef ECHO_BT_TO_USB
1402 Serial.print((char)ch);
1403 #endif
1404 PORTD &= ~16;
1405 delayMicroseconds(one_bit_write_duration);
1406 for (uint8_t i = 0; i < 8; i++)
1407 {
1408 if (ch & 1) PORTD |= 16;
1409 else PORTD &= ~16;
1410 ch >>= 1;
1411 delayMicroseconds(one_bit_write_duration);
1412 }
1413 PORTD |= 16;
1414 delayMicroseconds(one_bit_write_duration);
1415 delayMicroseconds(one_bit_write_duration);
1416 delayMicroseconds(one_bit_write_duration);
1417 delayMicroseconds(one_bit_write_duration);
1418 delayMicroseconds(one_bit_write_duration);
1419 }
1420
1421 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
1422 {
1423 uint16_t len;
1424 int16_t ch;
1425 do {
1426 ch = serial_read();
1427 if (ch == 13) continue;
1428 } while (ch == -1);
1429
1430 do {
1431 if ((ch != 13) && (ch != 10) && (ch != -1))
1432 {
1433 *(ln++) = ch;
1434 max_length--;
1435 len++;
1436 }
1437 ch = serial_read();
1438 } while ((ch != 13) && max_length);
1439 *ln = 0;
1440 return len;
1441 }
1442
1443 void serial_print_num(int32_t number)
1444 {
1445 if (number < 0)
1446 {
1447 serial_write('-');
1448 number = -number;
1449 }
1450 int32_t rad = 1;
1451 while (number / rad) rad *= 10;
1452 if (number > 0) rad /= 10;
1453 while (rad)
1454 {
1455 serial_write((char)('0' + (number / rad)));
1456 number -= (number / rad) * rad;
1457 rad /= 10;
1458 }
1459 }
1460
1461 void serial_print_char(char ch)
1462 {
1463 serial_write(ch);
1464 }
1465
1466 void serial_print(const char *str)
1467 {
1468 while (*str) serial_write(*(str++));
1469 }
1470
1471 void serial_println(const char *str)
1472 {
1473 serial_print(str);
1474 serial_write(13);
1475 serial_write(10);
1476 }
1477
1478 void serial_print_flash(const char *str)
1479 {
1480 int ln = strlen_P(str);
1481 for (int i = 0; i < ln; i++)
1482 serial_write(pgm_read_byte(str + i));
1483 }
1484
1485 void serial_println_flash(const char *str)
1486 {
1487 serial_print_flash(str);
1488 serial_write(13);
1489 serial_write(10);
1490 }
1491
1492 void serial_println_num(int32_t number)
1493 {
1494 serial_print_num(number);
1495 serial_println();
1496 }
1497
1498 void serial_println_char(char ch)
1499 {
1500 serial_write(ch);
1501 serial_println();
1502 }
1503
1504 void serial_println()
1505 {
1506 serial_write(13);
1507 serial_write(10);
1508 }
1509
1510 // nasleduje citanie z utltazvukoveho senzora
1511
1512 static volatile uint32_t pulse_start;
1513 static volatile uint8_t new_distance;
1514
1515 void init_ultrasonic()
1516 {
1517 pinMode(US_ECHO, INPUT);
1518 pinMode(US_TRIG, OUTPUT);
1519
1520 PCMSK0 |= 1; //PCINT0;
1521 PCIFR &= ~1; //PCIF0;
1522 PCICR |= 1; // PCIE0;
1523 }
1524
1525 ISR(PCINT0_vect)
1526 {
1527 if (PINB & 1) pulse_start = micros();
1528 else
1529 {
1530 distance = (int16_t)((micros() - pulse_start) / 58);
1531 new_distance = 1;
1532 }
1533 }
1534
1535 void start_distance_measurement()
1536 {
1537 distance = 10000;
1538 new_distance = 0;
1539 digitalWrite(US_TRIG, HIGH);
1540 delayMicroseconds(10);
1541 digitalWrite(US_TRIG, LOW);
1542 }
1543
1544 void wait_for_distance_measurement_to_complete()
1545 {
1546 uint8_t counter = 0;
1547 while ((counter < 20) && !new_distance)
1548 {
1549 delay(1);
1550 counter++;
1551 }
1552 if (counter == 20)
1553 {
1554 pinMode(US_ECHO, OUTPUT);
1555 digitalWrite(US_ECHO, HIGH);
1556 delayMicroseconds(10);
1557 digitalWrite(US_ECHO, LOW);
1558 pinMode(US_ECHO, INPUT);
1559 delayMicroseconds(5);
1560 distance = 10000;
1561 }
1562 }
1563
1564 int16_t measure_distance()
1565 {
1566 start_distance_measurement();
1567 wait_for_distance_measurement_to_complete();
1568 return distance;
1569 }
1570
1571 //-------------------------------- nasleduje prehravanie melodie a hranie cez timer2 v pozadi
1572 #define SIRENE_PORT PORTB
1573 #define SIRENE_DDR DDRB
1574 #define SIRENE_PIN 4
1575
1576 #define FIS3 2960
1577 #define G3 3136
1578
1579 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 };
1580
1581 //popcorn
1582 uint16_t dlzka_melodia[] = {0, 386, 26, 281, 217, 36};
1583 const uint8_t melodia1[] PROGMEM = { 252, 50, 149, 49,
1584 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,
1585 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,
1586 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,
1587 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,
1588 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,
1589 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,
1590 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,
1591 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 ,
1592 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,
1593 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,
1594 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 ,
1595 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
1596 };
1597
1598 //kohutik jarabi
1599 const uint8_t melodia2[] PROGMEM = { 252, 150, 119, 121, 173, 174, 124, 124, 124, 123, 171, 173, 123, 123, 123, 121, 169, 171, 121, 121, 121, 123, 171, 169, 119, 119 };
1600
1601 //kankan
1602 const uint8_t melodia3[] PROGMEM = { 252, 100,
1603 251, 1, 184, 1, 32, 126, 149, 251, 1, 184, 1, 32, 126, 149, 251, 1, 184, 1, 32, 126, 251, 1, 184, 1, 32, 126, 251, 1, 184, 1, 32, 126, 251, 1, 184, 1, 32, 126,
1604 64, 71, 71, 73, 71, 69, 69, 73, 74, 78, 81, 78, 78, 76, 251, 1, 184, 1, 32, 126, 78, 68, 68, 78, 76, 69, 69, 73, 73, 71, 73, 71, 85, 83, 85, 83,
1605 64, 71, 71, 73, 71, 69, 69, 73, 74, 78, 81, 78, 78, 76, 251, 1, 184, 1, 32, 126, 78, 68, 68, 78, 76, 69, 69, 73, 73, 71, 73, 71, 71, 69, 119,
1606 135, 131, 128, 126, 75, 76, 78, 80, 81, 76, 80, 76, 81, 76, 80, 76, 81, 76, 80, 76, 81, 76, 80, 76,
1607 251, 2, 11, 3, 16, 19, 251, 1, 4, 3, 16, 19,
1608 251, 1, 4, 3, 16, 19, 251, 1, 4, 3, 16, 19,
1609 251, 1, 4, 3, 16, 19, 251, 1, 4, 3, 16, 19,
1610 251, 1, 4, 3, 16, 19, 251, 1, 4, 3, 16, 19,
1611 174, 88, 91, 90, 88, 143, 143, 93, 95, 90, 91, 138, 138, 88, 91, 90, 88, 86, 86, 85, 83, 81, 79, 78, 76,
1612 174, 88, 91, 90, 88, 143, 143, 93, 95, 90, 91, 138, 138, 88, 91, 90, 88, 86, 93, 89, 90, 136,
1613 64, 71, 71, 73, 71, 69, 69, 73, 74, 78, 81, 78, 78, 76, 251, 1, 184, 1, 32, 126,
1614 78, 76, 126, 78, 76, 126, 78, 76, 126, 78, 76, 126, 78, 76, 126, 78, 76, 126,
1615 78, 76, 78, 76, 78, 76, 78, 76,
1616 131, 119, 119, 119, 169
1617 };
1618
1619 //labutie jazero
1620 const uint8_t melodia4[] PROGMEM = {
1621 252, 220, 66, 69, 73, 69, 66, 69, 73, 69, 66, 69, 73, 69, 66, 69, 73, 69,
1622 185, 78, 80, 81, 83, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 78, 81, 78, 73, 81, 178,
1623 99, 83, 81, 80,
1624 185, 78, 80, 81, 83, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 78, 81, 78, 73, 81, 178,
1625 149, 128, 130, 131, 133, 85, 86, 251, 6, 32, 3, 8, 86, 135, 86, 88, 251, 6, 224, 3, 8, 88, 136, 88, 90, 251, 7, 184, 3, 8, 90, 85, 81, 80, 78,
1626 130, 131, 133, 85, 86, 251, 6, 32, 3, 8, 86, 135, 86, 88, 251, 6, 224, 3, 8, 88, 136, 88, 90, 251, 7, 73, 3, 8, 86, 133, 86, 91, 251, 7, 184, 3, 8, 87, 251, 7, 184, 3, 8, 85,
1627 185, 78, 80, 81, 83, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 78, 81, 78, 73, 81, 178,
1628 99, 83, 81, 80,
1629 185, 78, 80, 81, 83, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 78, 81, 78, 73, 81, 178
1630 };
1631
1632 //let it be https://www.musicnotes.com/sheetmusic/mtd.asp?ppn=MN0101556
1633 const uint8_t melodia5[] PROGMEM = { 252, 200, 26, 26, 76, 26, 253, 78, 73, 76, 76, 31, 253, 83, 35, 85, 253, 85,
1634 83, 83, 81, 131, 35, 253, 85, 86, 35, 35, 85, 83, 99, 35, 33, 83, 81, 181
1635 };
1636
1637 volatile int16_t music_speed = 800 / 16;
1638 volatile const uint8_t *current_note ;
1639 volatile uint16_t notes_remaining;
1640 uint8_t dotted_note = 0;
1641
1642 void zahraj_melodiu(uint8_t cislo)
1643 {
1644 if (cislo == 0) {
1645 zastav_melodiu();
1646 return;
1647 }
1648
1649 if (cislo == 1) current_note = melodia1;
1650 else if (cislo == 2) current_note = melodia2;
1651 else if (cislo == 3) current_note = melodia3;
1652 else if (cislo == 4) current_note = melodia4;
1653 else if (cislo == 5) current_note = melodia5;
1654 notes_remaining = dlzka_melodia[cislo];
1655
1656 next_note();
1657 }
1658
1659 void next_note()
1660 {
1661 uint16_t freq = 0, dur = 0;
1662 if (!notes_remaining) return;
1663 otto_translate_tone_flash(&freq, &dur);
1664 tone2(freq, dur);
1665 }
1666
1667 void otto_translate_tone_flash(uint16_t *freq, uint16_t *del)
1668 {
1669 do {
1670 uint8_t n = pgm_read_byte(current_note);
1671 if (n > 249)
1672 {
1673 if (n == 251)
1674 {
1675 current_note++;
1676 uint8_t f1 = pgm_read_byte(current_note);
1677 current_note++;
1678 uint8_t f2 = pgm_read_byte(current_note);
1679 current_note++;
1680 uint8_t d1 = pgm_read_byte(current_note);
1681 current_note++;
1682 uint8_t d2 = pgm_read_byte(current_note);
1683 *freq = (f1 << 8) + f2;
1684 *del = (music_speed * 16 * (long)d1) / d2;
1685 notes_remaining -= 4;
1686 }
1687 else if (n == 252)
1688 {
1689 current_note++;
1690 music_speed = pgm_read_byte(current_note);
1691 current_note++;
1692 notes_remaining -= 2;
1693 continue;
1694 }
1695 else if (n == 253)
1696 {
1697 dotted_note = 1;
1698 current_note++;
1699 notes_remaining--;
1700 continue;
1701 }
1702 else if (n == 254)
1703 {
1704 *freq = FIS3;
1705 *del = music_speed;
1706 }
1707 else if (n == 255)
1708 {
1709 *freq = G3;
1710 *del = music_speed;
1711 }
1712 }
1713 else
1714 {
1715 uint8_t len = n / 50;
1716 *del = music_speed;
1717 while (len--) *del *= 2;
1718 n = n % 50;
1719 if (n != 49)
1720 {
1721 uint8_t octave = (n + 5) / 12;
1722 n = (n + 5) % 12;
1723 float ffreq = octave_4[n];
1724 octave = 4 - octave;
1725 while (octave > 0)
1726 {
1727 ffreq /= 2.0;
1728 octave--;
1729 }
1730 *freq = (uint16_t)ffreq;
1731 }
1732 else *freq = 0;
1733 }
1734 notes_remaining--;
1735 current_note++;
1736 break;
1737 } while (1);
1738 if (dotted_note)
1739 {
1740 *del += (*del) / 2;
1741 dotted_note = 0;
1742 }
1743 }
1744
1745 static volatile uint8_t tone2_state;
1746 static volatile uint8_t tone2_pause;
1747 static volatile uint32_t tone2_len;
1748
1749 void init_tone2()
1750 {
1751 notes_remaining = 0;
1752 tone2_pause = 0;
1753 dotted_note = 0;
1754 TCCR2A = 2;
1755 TCCR2B = 0;
1756 TIMSK2 = 2;
1757 SIRENE_DDR |= (1 << SIRENE_PIN);
1758 }
1759
1760 ISR(TIMER2_COMPA_vect)
1761 {
1762 if (!tone2_pause)
1763 {
1764 if (tone2_state)
1765 {
1766 SIRENE_PORT |= (1 << SIRENE_PIN);
1767 tone2_state = 0;
1768 }
1769 else
1770 {
1771 SIRENE_PORT &= ~(1 << SIRENE_PIN);
1772 tone2_state = 1;
1773 }
1774 }
1775 if ((--tone2_len) == 0)
1776 {
1777 TCCR2B = 0;
1778 tone2_pause = 0;
1779 next_note();
1780 }
1781 }
1782
1783 void tone2(uint16_t freq, uint16_t duration)
1784 {
1785 uint32_t period = ((uint32_t)1000000) / (uint32_t)freq;
1786
1787 if (freq >= 977) // prescaler 32
1788 {
1789 tone2_state = 0;
1790 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1791 if (tone2_len == 0) tone2_len++;
1792 TCNT2 = 0;
1793 OCR2A = (uint8_t) (250000 / (uint32_t)freq);
1794 TCCR2B = 3;
1795 }
1796 else if (freq >= 488) // prescaler 64
1797 {
1798 tone2_state = 0;
1799 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1800 if (tone2_len == 0) tone2_len++;
1801 TCNT2 = 0;
1802 OCR2A = (uint8_t) (125000 / (uint32_t)freq);
1803 TCCR2B = 4;
1804 }
1805 else if (freq >= 244) // prescaler 128
1806 {
1807 tone2_state = 0;
1808 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1809 if (tone2_len == 0) tone2_len++;
1810 TCNT2 = 0;
1811 OCR2A = (uint8_t) (62500 / (uint32_t)freq);
1812 TCCR2B = 5;
1813 }
1814 else if (freq >= 122) //prescaler 256
1815 {
1816 tone2_state = 0;
1817 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1818 if (tone2_len == 0) tone2_len++;
1819 TCNT2 = 0;
1820 OCR2A = (uint8_t) (31250 / (uint32_t)freq);
1821 TCCR2B = 6;
1822 }
1823 else if (freq >= 30) //prescaler 1024
1824 {
1825 tone2_state = 0;
1826 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1827 if (tone2_len == 0) tone2_len++;
1828
1829 TCNT2 = 0;
1830 OCR2A = (uint8_t) (7813 / (uint32_t)freq);
1831 TCCR2B = 7;
1832 }
1833 else if (freq == 0)
1834 {
1835 tone2_pause = 1;
1836 tone2_state = 0;
1837 period = 1000000 / 500;
1838 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1839 TCNT2 = 0;
1840 OCR2A = (uint8_t) (125000 / (uint32_t)500);
1841 TCCR2B = 4;
1842 }
1843 else
1844 {
1845 TCCR2B = 0;
1846 }
1847 }
1848
1849 void zastav_melodiu()
1850 {
1851 notes_remaining = 0;
1852 }