Difference between revisions of "Otto - riadiaci program v.1"
m |
m |
||
Line 1: | Line 1: | ||
− | Program umožňuje priame riadenie robota cez BlueTooth | + | Program umožňuje priame riadenie robota (aj cez BlueTooth). |
+ | Zatiaľ musíme pripájať BT na piny 0 a 1 (namiesto 2 a 4) a počas programovania vytiahnuť káblik označený TXD na strane BT modulu. | ||
{|class="wikitable" | {|class="wikitable" | ||
Line 15: | Line 16: | ||
|- | |- | ||
|k/m || pravá päta || 3 | |k/m || pravá päta || 3 | ||
+ | |- | ||
+ | |1/9 || zníž/zvýš rýchlosť pohybu || - | ||
|- | |- | ||
|Tx || BT|| 4 | |Tx || BT|| 4 | ||
Line 28: | Line 31: | ||
Ďalej umožňuje | Ďalej umožňuje | ||
− | *kalibrovať strednú polohu servomotorov (pomocou klávesu '''H'''), | + | *kalibrovať strednú polohu servomotorov (pomocou klávesu '''H'''), zobrazenie zadanej kalibrácie: klávesu '''G''' |
− | *pozdraví po klávese '''-''', | + | *kalibráciu je možné zapísať do trvalej pamäte EEPROM pomocou klávesu '''E''' (používať opatrne!), po zapnutí sa automaticky načíta |
+ | *pozdraví po klávese '''-''', ruky dá hore na kláves '''R''' | ||
*načíta choreografiu po klávese '''@''', | *načíta choreografiu po klávese '''@''', | ||
*zobrazí načítanú choreografiu na kláves '''?''' a | *zobrazí načítanú choreografiu na kláves '''?''' a | ||
Line 41: | Line 45: | ||
<syntaxhighlight lang="C" line=line> | <syntaxhighlight lang="C" line=line> | ||
− | #include <SoftwareSerial.h> | + | //#include <SoftwareSerial.h> |
#include <Servo.h> | #include <Servo.h> | ||
+ | #include <EEPROM.h> | ||
+ | |||
+ | #define pc Serial | ||
#define US_TRIG 7 | #define US_TRIG 7 | ||
Line 55: | Line 62: | ||
#define CHOREO_LEN 200 | #define CHOREO_LEN 200 | ||
− | // | + | // cisla pinov, kde su zapojene servo motory |
− | + | #define PIN_SERVO_LAVA_RUKA 10 | |
− | + | #define PIN_SERVO_PRAVA_RUKA 11 | |
− | + | #define PIN_SERVO_LAVA_NOHA 9 | |
− | + | #define PIN_SERVO_PRAVA_NOHA 6 | |
− | + | #define PIN_SERVO_LAVA_PATA 5 | |
− | + | #define PIN_SERVO_PRAVA_PATA 3 | |
− | // | + | |
+ | #define S1 3 | ||
+ | #define S2 5 | ||
+ | #define S3 6 | ||
+ | #define S4 9 | ||
+ | #define S5 10 | ||
+ | #define S6 11 | ||
+ | #define SIRENA 12 | ||
+ | |||
+ | //maximalna dlzka choreografie | ||
+ | #define CHOREO_LEN 200 | ||
− | // | + | // tu su serva cislovane 1-6 |
− | #define SERVO_LAVA_RUKA | + | #define SERVO_LAVA_RUKA 5 |
− | #define SERVO_PRAVA_RUKA | + | #define SERVO_PRAVA_RUKA 6 |
− | #define SERVO_LAVA_NOHA | + | #define SERVO_LAVA_NOHA 4 |
− | #define SERVO_PRAVA_NOHA | + | #define SERVO_PRAVA_NOHA 3 |
− | #define SERVO_LAVA_PATA | + | #define SERVO_LAVA_PATA 2 |
− | #define SERVO_PRAVA_PATA | + | #define SERVO_PRAVA_PATA 1 |
// ak su niektore serva naopak, je tu jednotka | // ak su niektore serva naopak, je tu jednotka | ||
Line 78: | Line 95: | ||
char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' }; | char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' }; | ||
// co robia jednotlive znaky (znamienko urcuje smer) | // co robia jednotlive znaky (znamienko urcuje smer) | ||
− | int8_t zmeny[] = { | + | int8_t zmeny[] = {SERVO_LAVA_RUKA, -SERVO_LAVA_RUKA, |
− | + | SERVO_PRAVA_RUKA, -SERVO_PRAVA_RUKA, | |
-SERVO_LAVA_NOHA, SERVO_LAVA_NOHA, | -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA, | ||
-SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA, | -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA, | ||
Line 86: | Line 103: | ||
// sem si mozno ulozit svoju kalibraciu | // sem si mozno ulozit svoju kalibraciu | ||
+ | //uint8_t prednastavena_kalibracia[] = { 78, 69, 83, 80, 50, 67 }; | ||
uint8_t prednastavena_kalibracia[] = { 90, 90, 90, 90, 90, 90 }; | uint8_t prednastavena_kalibracia[] = { 90, 90, 90, 90, 90, 90 }; | ||
Servo s[6]; | Servo s[6]; | ||
− | SoftwareSerial pc(BT_TX, BT_RX); | + | //SoftwareSerial pc(BT_TX, BT_RX); |
uint16_t ch_time[CHOREO_LEN]; | uint16_t ch_time[CHOREO_LEN]; | ||
Line 101: | Line 119: | ||
void setup() { | void setup() { | ||
randomSeed(analogRead(1)); | randomSeed(analogRead(1)); | ||
− | s[0].attach( | + | s[0].attach(PIN_SERVO_PRAVA_PATA); |
− | s[1].attach( | + | s[1].attach(PIN_SERVO_LAVA_PATA); |
− | s[2].attach( | + | s[2].attach(PIN_SERVO_PRAVA_NOHA); |
− | s[3].attach( | + | s[3].attach(PIN_SERVO_LAVA_NOHA); |
− | s[4].attach( | + | s[4].attach(PIN_SERVO_PRAVA_RUKA); |
− | s[5].attach( | + | s[5].attach(PIN_SERVO_LAVA_RUKA); |
+ | precitaj_kalibraciu_z_EEPROM(); | ||
for (int i = 0; i < 6; i++) | for (int i = 0; i < 6; i++) | ||
{ | { | ||
Line 113: | Line 132: | ||
s[i].write(stav[i]); | s[i].write(stav[i]); | ||
} | } | ||
− | pc.begin(9600); | + | // pc.begin(9600); |
Serial.begin(9600); | Serial.begin(9600); | ||
ch_len = 0; | ch_len = 0; | ||
Line 119: | Line 138: | ||
ahoj(); | ahoj(); | ||
ruky(); | ruky(); | ||
+ | } | ||
+ | |||
+ | void precitaj_kalibraciu_z_EEPROM() | ||
+ | { | ||
+ | uint8_t value = EEPROM.read(1); | ||
+ | if (value != '~') return; | ||
+ | for (int i = 2; i < 8; i++) | ||
+ | prednastavena_kalibracia[i - 2] = EEPROM.read(i); | ||
+ | } | ||
+ | |||
+ | void zapis_kalibraciu_do_EEPROM() | ||
+ | { | ||
+ | pc.print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: "); | ||
+ | while (!pc.available()); | ||
+ | char odpoved = pc.read(); | ||
+ | pc.println(odpoved); | ||
+ | if (odpoved == 'Y') | ||
+ | { | ||
+ | EEPROM.write(1, '~'); | ||
+ | for (int i = 2; i < 8; i++) | ||
+ | EEPROM.write(i, kalib[i - 2]); | ||
+ | pc.println("ok"); | ||
+ | } | ||
} | } | ||
Line 160: | Line 202: | ||
if (random(10) > 4) tone(SIRENA, 1357, 50); | if (random(10) > 4) tone(SIRENA, 1357, 50); | ||
else tone(SIRENA, 1047, 50); | else tone(SIRENA, 1047, 50); | ||
+ | } | ||
+ | |||
+ | void nastav_koncatinu(int8_t servo, uint8_t poloha) | ||
+ | { | ||
+ | int8_t srv = (servo > 0)?servo:-servo; | ||
+ | srv--; | ||
+ | poloha += prednastavena_kalibracia[srv] - 90; | ||
+ | if (poloha > 180) poloha = 180; | ||
+ | if (poloha < 0) poloha = 0; | ||
+ | stav[srv] = poloha; | ||
+ | s[srv].write(stav[srv]); | ||
} | } | ||
Line 277: | Line 330: | ||
{ | { | ||
delay(ch_time[i]); | delay(ch_time[i]); | ||
− | + | nastav_koncatinu(ch_servo[i], ch_val[i]); | |
} | } | ||
+ | if (ch_len > 0) stav[ch_servo[ch_len - 1]] = ch_val[ch_len - 1]; | ||
pipni(); | pipni(); | ||
} | } | ||
Line 313: | Line 367: | ||
pc.println("ok"); | pc.println("ok"); | ||
pipni(); | pipni(); | ||
+ | } | ||
+ | |||
+ | |||
+ | void vypis_kalibraciu() | ||
+ | { | ||
+ | for (int i = 0; i < 6; i++) { pc.print(kalib[i]); pc.print(" "); } | ||
+ | pc.println(); | ||
} | } | ||
void nacitaj_kalibraciu() | void nacitaj_kalibraciu() | ||
{ | { | ||
− | |||
int tm; | int tm; | ||
for (int i = 0; i < 6; i++) | for (int i = 0; i < 6; i++) | ||
kalib[i] = nacitajCislo(); | kalib[i] = nacitajCislo(); | ||
− | + | vypis_kalibraciu(); | |
pc.println("ok"); | pc.println("ok"); | ||
pipni(); | pipni(); | ||
Line 352: | Line 412: | ||
else if (z == ' ') reset(); | else if (z == ' ') reset(); | ||
else if (z == 'H') kalibruj(); | else if (z == 'H') kalibruj(); | ||
+ | else if (z == 'G') vypis_kalibraciu(); | ||
else if (z == 'L') nacitaj_kalibraciu(); | else if (z == 'L') nacitaj_kalibraciu(); | ||
+ | else if (z == 'E') zapis_kalibraciu_do_EEPROM(); | ||
+ | else if (z == 'R') ruky(); | ||
else if (z == '9') zvys_krok(); | else if (z == '9') zvys_krok(); | ||
else if (z == '1') zniz_krok(); | else if (z == '1') zniz_krok(); |
Revision as of 06:05, 2 August 2018
Program umožňuje priame riadenie robota (aj cez BlueTooth). Zatiaľ musíme pripájať BT na piny 0 a 1 (namiesto 2 a 4) a počas programovania vytiahnuť káblik označený TXD na strane BT modulu.
klaves | servo | pin |
---|---|---|
a/q | ľavá ruka | 10 |
;/p | pravá ruka | 11 |
z/x | ľavá noha | 9 |
,/. | pravá noha | 6 |
d/c | ľavá päta | 5 |
k/m | pravá päta | 3 |
1/9 | zníž/zvýš rýchlosť pohybu | - |
Tx | BT | 4 |
Rx | BT | 2 |
TRIG | UZ | 7 |
ECHO | UZ | 8 |
Sirena | 12 |
Ďalej umožňuje
- kalibrovať strednú polohu servomotorov (pomocou klávesu H), zobrazenie zadanej kalibrácie: klávesu G
- kalibráciu je možné zapísať do trvalej pamäte EEPROM pomocou klávesu E (používať opatrne!), po zapnutí sa automaticky načíta
- pozdraví po klávese -, ruky dá hore na kláves R
- načíta choreografiu po klávese @,
- zobrazí načítanú choreografiu na kláves ? a
- zatancuje načítanú choreografiu - kláves t.
- Na klávesách 3-6 je možné nastaviť rôznu kombináciu (funkcie kombinacia1() - kombinacia4()).
- Medzera resetne všetky servo motory do strednej polohy.
Pre komunikáciu na pinoch 0,1 pomocou bežného sériového portu treba pridať riadok
#define pc Serial
a zakomentovať riadky pc.begin(...);
a SoftwareSerial pc(...)
1 //#include <SoftwareSerial.h>
2 #include <Servo.h>
3 #include <EEPROM.h>
4
5 #define pc Serial
6
7 #define US_TRIG 7
8 #define US_ECHO 8
9
10 #define BT_RX 2
11 #define BT_TX 4
12
13 #define SIRENA 12
14
15 //maximalna dlzka choreografie
16 #define CHOREO_LEN 200
17
18 // cisla pinov, kde su zapojene servo motory
19 #define PIN_SERVO_LAVA_RUKA 10
20 #define PIN_SERVO_PRAVA_RUKA 11
21 #define PIN_SERVO_LAVA_NOHA 9
22 #define PIN_SERVO_PRAVA_NOHA 6
23 #define PIN_SERVO_LAVA_PATA 5
24 #define PIN_SERVO_PRAVA_PATA 3
25
26 #define S1 3
27 #define S2 5
28 #define S3 6
29 #define S4 9
30 #define S5 10
31 #define S6 11
32 #define SIRENA 12
33
34 //maximalna dlzka choreografie
35 #define CHOREO_LEN 200
36
37 // tu su serva cislovane 1-6
38 #define SERVO_LAVA_RUKA 5
39 #define SERVO_PRAVA_RUKA 6
40 #define SERVO_LAVA_NOHA 4
41 #define SERVO_PRAVA_NOHA 3
42 #define SERVO_LAVA_PATA 2
43 #define SERVO_PRAVA_PATA 1
44
45 // ak su niektore serva naopak, je tu jednotka
46 uint8_t servo_invertovane[6] = {0, 0, 1, 1, 0, 1};
47
48 // znaky, ktorymi sa ovladaju jednotlive stupne volnosti
49 char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' };
50 // co robia jednotlive znaky (znamienko urcuje smer)
51 int8_t zmeny[] = {SERVO_LAVA_RUKA, -SERVO_LAVA_RUKA,
52 SERVO_PRAVA_RUKA, -SERVO_PRAVA_RUKA,
53 -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA,
54 -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA,
55 SERVO_LAVA_PATA, -SERVO_LAVA_PATA,
56 SERVO_PRAVA_PATA, -SERVO_PRAVA_PATA };
57
58 // sem si mozno ulozit svoju kalibraciu
59 //uint8_t prednastavena_kalibracia[] = { 78, 69, 83, 80, 50, 67 };
60 uint8_t prednastavena_kalibracia[] = { 90, 90, 90, 90, 90, 90 };
61
62 Servo s[6];
63 //SoftwareSerial pc(BT_TX, BT_RX);
64
65 uint16_t ch_time[CHOREO_LEN];
66 uint8_t ch_servo[CHOREO_LEN];
67 uint8_t ch_val[CHOREO_LEN];
68 int ch_len;
69 uint8_t kalib[6];
70 int stav[6];
71 int krok;
72
73 void setup() {
74 randomSeed(analogRead(1));
75 s[0].attach(PIN_SERVO_PRAVA_PATA);
76 s[1].attach(PIN_SERVO_LAVA_PATA);
77 s[2].attach(PIN_SERVO_PRAVA_NOHA);
78 s[3].attach(PIN_SERVO_LAVA_NOHA);
79 s[4].attach(PIN_SERVO_PRAVA_RUKA);
80 s[5].attach(PIN_SERVO_LAVA_RUKA);
81 precitaj_kalibraciu_z_EEPROM();
82 for (int i = 0; i < 6; i++)
83 {
84 kalib[i] = prednastavena_kalibracia[i];
85 stav[i] = kalib[i];
86 s[i].write(stav[i]);
87 }
88 // pc.begin(9600);
89 Serial.begin(9600);
90 ch_len = 0;
91 krok = 1;
92 ahoj();
93 ruky();
94 }
95
96 void precitaj_kalibraciu_z_EEPROM()
97 {
98 uint8_t value = EEPROM.read(1);
99 if (value != '~') return;
100 for (int i = 2; i < 8; i++)
101 prednastavena_kalibracia[i - 2] = EEPROM.read(i);
102 }
103
104 void zapis_kalibraciu_do_EEPROM()
105 {
106 pc.print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
107 while (!pc.available());
108 char odpoved = pc.read();
109 pc.println(odpoved);
110 if (odpoved == 'Y')
111 {
112 EEPROM.write(1, '~');
113 for (int i = 2; i < 8; i++)
114 EEPROM.write(i, kalib[i - 2]);
115 pc.println("ok");
116 }
117 }
118
119 void pipni()
120 {
121 tone(SIRENA, 1568, 50);
122 delay(100);
123 tone(SIRENA, 1357, 50);
124 }
125
126 void ruky()
127 {
128 int odloz_krok = krok;
129 delay(500);
130 krok = 90;
131 pohyb(SERVO_LAVA_RUKA);
132 pohyb(SERVO_PRAVA_RUKA);
133 delay(1000);
134 krok = 180;
135 pohyb(-SERVO_LAVA_RUKA);
136 pohyb(-SERVO_PRAVA_RUKA);
137 delay(1000);
138 krok = odloz_krok;
139 pipni();
140 }
141
142 void ahoj()
143 {
144 tone(SIRENA, 1568, 50);
145 delay(70);
146 tone(SIRENA, 1175, 30);
147 delay(50);
148 tone(SIRENA, 880, 30);
149 delay(50);
150 tone(SIRENA, 1047, 50);
151 delay(70);
152 tone(SIRENA, 1245, 30);
153 delay(150);
154 tone(SIRENA, 1568, 50);
155 delay(100);
156 if (random(10) > 4) tone(SIRENA, 1357, 50);
157 else tone(SIRENA, 1047, 50);
158 }
159
160 void nastav_koncatinu(int8_t servo, uint8_t poloha)
161 {
162 int8_t srv = (servo > 0)?servo:-servo;
163 srv--;
164 poloha += prednastavena_kalibracia[srv] - 90;
165 if (poloha > 180) poloha = 180;
166 if (poloha < 0) poloha = 0;
167 stav[srv] = poloha;
168 s[srv].write(stav[srv]);
169 }
170
171 void pohyb(int8_t servo)
172 {
173 int8_t srv = (servo > 0)?servo:-servo;
174 srv--;
175 if (servo_invertovane[srv]) servo = -servo;
176 if (servo > 0)
177 {
178 if (stav[srv] <= 180 - krok) stav[srv] += krok;
179 else stav[srv] = 180;
180 s[srv].write(stav[srv]);
181 }
182 else if (servo < 0)
183 {
184 if (stav[srv] >= krok) stav[srv] -= krok;
185 else stav[srv] = 0;
186 s[srv].write(stav[srv]);
187 }
188 }
189
190 uint8_t pohyb_znakom(char z)
191 {
192 for (int i = 0; i < 12; i++)
193 {
194 if (z == znaky_zmien[i])
195 {
196 int8_t servo = zmeny[i];
197 pohyb(servo);
198 }
199 }
200 }
201
202 void kombinacia1()
203 {
204 pohyb(SERVO_LAVA_NOHA);
205 pohyb(-SERVO_PRAVA_PATA);
206 }
207
208 void kombinacia2()
209 {
210 pohyb(SERVO_PRAVA_NOHA);
211 pohyb(-SERVO_LAVA_PATA);
212 }
213
214 void kombinacia3()
215 {
216 pohyb(SERVO_LAVA_RUKA);
217 pohyb(SERVO_PRAVA_RUKA);
218 }
219
220 void kombinacia4()
221 {
222 pohyb(-SERVO_LAVA_RUKA);
223 pohyb(-SERVO_PRAVA_RUKA);
224 }
225
226 int pohyb_kombinacia(char z)
227 {
228 if (z == '3') kombinacia1();
229 else if (z == '4') kombinacia2();
230 else if (z == '5') kombinacia3();
231 else if (z == '6') kombinacia4();
232 else return 0;
233 return 1;
234 }
235
236 int nacitajCislo()
237 {
238 int num = 0;
239 int z;
240 do {
241 z = pc.read();
242 if (z == '#') while (z != 13) z = pc.read();
243 } while ((z < '0') || (z > '9'));
244 while ((z >= '0') && (z <= '9'))
245 {
246 num *= 10;
247 num += (z - '0');
248 do { z = pc.read(); if (z == -1) delayMicroseconds(10); } while (z < 0);
249 }
250 return num;
251 }
252
253 void nacitaj_choreografiu()
254 {
255 ch_len = 0;
256 int tm;
257 do {
258 tm = nacitajCislo();
259 ch_time[ch_len] = tm;
260 ch_servo[ch_len] = nacitajCislo();
261 ch_val[ch_len] = nacitajCislo();
262 ch_len++;
263 if (ch_len == CHOREO_LEN) break;
264 } while (tm > 0);
265 pipni();
266 }
267
268 void vypis_choreografiu()
269 {
270 for (int i = 0; i < ch_len; i++)
271 {
272 pc.print(ch_time[i]);
273 pc.print(" ");
274 pc.print(ch_servo[i]);
275 pc.print(" ");
276 pc.println(ch_val[i]);
277 }
278 pipni();
279 }
280
281 void zatancuj_choreografiu()
282 {
283 for (int i = 0; i < ch_len; i++)
284 {
285 delay(ch_time[i]);
286 nastav_koncatinu(ch_servo[i], ch_val[i]);
287 }
288 if (ch_len > 0) stav[ch_servo[ch_len - 1]] = ch_val[ch_len - 1];
289 pipni();
290 }
291
292 void reset()
293 {
294 for (int i = 0; i < 6; i++)
295 {
296 stav[i] = kalib[i];
297 s[i].write(kalib[i]);
298 }
299 pipni();
300 }
301
302 void kalibruj()
303 {
304 for (int i = 0; i < 6; i++)
305 {
306 pc.print(i);
307 pc.print(" (+/-/ENTER): ");
308 pc.println(kalib[i]);
309 s[i].write(kalib[i]);
310 char z;
311 do {
312 z = pc.read();
313 if ((z == '+') && (kalib[i] < 180)) { kalib[i]++; pc.print(kalib[i]); pc.print('\r'); s[i].write(kalib[i]); }
314 else if ((z == '-') && (kalib[i] > 0)) { kalib[i]--; pc.print(kalib[i]); pc.print('\r'); s[i].write(kalib[i]); }
315 } while (z != 13);
316 pc.print(i);
317 pc.print(": ");
318 pc.println(kalib[i]);
319 }
320 for (int i = 0; i < 6; i++) { pc.print(kalib[i]); pc.print(" "); }
321 pc.println("ok");
322 pipni();
323 }
324
325
326 void vypis_kalibraciu()
327 {
328 for (int i = 0; i < 6; i++) { pc.print(kalib[i]); pc.print(" "); }
329 pc.println();
330 }
331
332 void nacitaj_kalibraciu()
333 {
334 int tm;
335 for (int i = 0; i < 6; i++)
336 kalib[i] = nacitajCislo();
337 vypis_kalibraciu();
338 pc.println("ok");
339 pipni();
340 }
341
342 void zvys_krok()
343 {
344 if (krok < 180) krok++;
345 pc.print("krok: ");
346 pc.println(krok);
347 }
348
349 void zniz_krok()
350 {
351 if (krok > 0) krok--;
352 pc.print("krok: ");
353 pc.println(krok);
354 }
355
356 void loop() {
357 if (pc.available())
358 {
359 char z = pc.read();
360 if (pohyb_znakom(z)) return;
361 else if (pohyb_kombinacia(z)) return;
362 else if (z == '@') nacitaj_choreografiu();
363 else if (z == '?') vypis_choreografiu();
364 else if (z == 't') zatancuj_choreografiu();
365 else if (z == '-') ahoj();
366 else if (z == ' ') reset();
367 else if (z == 'H') kalibruj();
368 else if (z == 'G') vypis_kalibraciu();
369 else if (z == 'L') nacitaj_kalibraciu();
370 else if (z == 'E') zapis_kalibraciu_do_EEPROM();
371 else if (z == 'R') ruky();
372 else if (z == '9') zvys_krok();
373 else if (z == '1') zniz_krok();
374 }
375 }