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