Otto - riadiaci program v.1

From DT^2
Revision as of 07:34, 2 August 2018 by Admin (talk | contribs)
Jump to: navigation, search

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)
  • kalibrovať limity pre všetky stupne voľnosti (pomocou klávesu J), zobrazenie celej kalibrácie: kláves G
  • celú 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 uint8_t dolny_limit[] = {0, 0, 0, 0, 0, 0, 0, 0};
 63 uint8_t horny_limit[] = {180, 180, 180, 180, 180, 180};
 64 
 65 Servo s[6];
 66 //SoftwareSerial pc(BT_TX, BT_RX);
 67 
 68 uint16_t ch_time[CHOREO_LEN];
 69 uint8_t ch_servo[CHOREO_LEN];
 70 uint8_t ch_val[CHOREO_LEN];
 71 int ch_len;
 72 uint8_t kalib[6];
 73 int stav[6];
 74 int krok;
 75 
 76 void setup() {
 77   randomSeed(analogRead(1));
 78   s[0].attach(PIN_SERVO_PRAVA_PATA);
 79   s[1].attach(PIN_SERVO_LAVA_PATA);
 80   s[2].attach(PIN_SERVO_PRAVA_NOHA);
 81   s[3].attach(PIN_SERVO_LAVA_NOHA);
 82   s[4].attach(PIN_SERVO_PRAVA_RUKA);
 83   s[5].attach(PIN_SERVO_LAVA_RUKA);
 84   precitaj_kalibraciu_z_EEPROM();
 85   for (int i = 0; i < 6; i++)
 86   {
 87     kalib[i] = prednastavena_kalibracia[i];
 88     stav[i] = kalib[i];
 89     s[i].write(stav[i]);
 90   }
 91   // pc.begin(9600);
 92   Serial.begin(9600);
 93   ch_len = 0;
 94   krok = 7;
 95   ahoj();
 96   ruky();
 97 }
 98 
 99 void precitaj_kalibraciu_z_EEPROM()
100 {
101   uint8_t value = EEPROM.read(1);
102   if (value != '~') return;
103   for (int i = 2; i < 8; i++)
104     prednastavena_kalibracia[i - 2] = EEPROM.read(i);
105   for (int i = 0; i < 6; i++)
106     dolny_limit[i] = EEPROM.read(i + 9);
107   for (int i = 0; i < 6; i++)
108     horny_limit[i] = EEPROM.read(i + 15);    
109 }
110 
111 void zapis_kalibraciu_do_EEPROM()
112 {
113   pc.print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
114   while (!pc.available());
115   char odpoved = pc.read();
116   pc.println(odpoved);
117   if (odpoved == 'Y') 
118   {
119     EEPROM.write(1, '~');
120     for (int i = 2; i < 8; i++)
121       EEPROM.write(i, kalib[i - 2]);
122     for (int i = 0; i < 6; i++)
123       EEPROM.write(9 + i, dolny_limit[i]);
124     for (int i = 0; i < 6; i++)
125       EEPROM.write(15 + i, horny_limit[i]);
126     pc.println("ok");
127   }
128 }
129 
130 void pipni()
131 {
132   tone(SIRENA, 1568, 50);
133   delay(100);
134   tone(SIRENA, 1357, 50);
135 }
136 
137 void ruky()
138 {
139   int odloz_krok = krok;
140   delay(500);
141   krok = 90;
142   pohyb(SERVO_LAVA_RUKA);    
143   pohyb(SERVO_PRAVA_RUKA);
144   delay(1000);
145   krok = 180;
146   pohyb(-SERVO_LAVA_RUKA);    
147   pohyb(-SERVO_PRAVA_RUKA);
148   delay(1000);
149   krok = odloz_krok;
150   pipni();
151 }
152 
153 void ahoj()
154 {
155   tone(SIRENA, 1568, 50);
156   delay(70);
157   tone(SIRENA, 1175, 30);
158   delay(50);
159   tone(SIRENA, 880, 30);
160   delay(50);
161   tone(SIRENA, 1047, 50);
162   delay(70);
163   tone(SIRENA, 1245, 30);
164   delay(150);
165   tone(SIRENA, 1568, 50);
166   delay(100);
167   if (random(10) > 4) tone(SIRENA, 1357, 50);
168   else tone(SIRENA, 1047, 50);
169 }
170 
171 void nastav_koncatinu(int8_t servo, uint8_t poloha)
172 {
173   int8_t srv = (servo > 0)?servo:-servo;
174   srv--;
175   poloha += kalib[srv] - 90;
176   if (poloha > 180) poloha = 180;
177   if (poloha < 0) poloha = 0;
178   stav[srv] = poloha;
179   s[srv].write(stav[srv]);
180 }
181 
182 void pohyb(int8_t servo)
183 {
184   int8_t srv = (servo > 0)?servo:-servo;
185   srv--;
186   if (servo_invertovane[srv]) servo = -servo;
187   if (servo > 0)
188   {
189     if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
190     else stav[srv] = horny_limit[srv];
191     s[srv].write(stav[srv]);
192   }
193   else if (servo < 0)
194   {
195     if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok; 
196     else stav[srv] = dolny_limit[srv];
197     s[srv].write(stav[srv]);      
198   }
199 }
200 
201 uint8_t pohyb_znakom(char z)
202 {
203   for (int i = 0; i < 12; i++)
204   {
205     if (z == znaky_zmien[i])
206     {
207       int8_t servo = zmeny[i];
208       pohyb(servo);
209     }
210   }
211 }
212 
213 void kombinacia1()
214 {
215   pohyb(SERVO_LAVA_NOHA);
216   pohyb(-SERVO_PRAVA_PATA);
217 }
218 
219 void kombinacia2()
220 {
221   pohyb(SERVO_PRAVA_NOHA);
222   pohyb(-SERVO_LAVA_PATA);
223 }
224 
225 void kombinacia3()
226 {
227    pohyb(SERVO_LAVA_RUKA); 
228    pohyb(SERVO_PRAVA_RUKA); 
229 }
230 
231 void kombinacia4()
232 {
233    pohyb(-SERVO_LAVA_RUKA); 
234    pohyb(-SERVO_PRAVA_RUKA); 
235 }
236 
237 int pohyb_kombinacia(char z)
238 {
239   if (z == '3') kombinacia1();
240   else if (z == '4') kombinacia2();
241   else if (z == '5') kombinacia3();
242   else if (z == '6') kombinacia4();
243   else return 0;
244   return 1;
245 }
246 
247 int nacitajCislo()
248 {
249   int num = 0;
250   int z;
251   do {
252     z = pc.read();
253     if (z == '#') while (z != 13) z = pc.read();
254   } while ((z < '0') || (z > '9'));
255   while ((z >= '0') && (z <= '9'))
256   {
257     num *= 10;
258     num += (z - '0');
259     do { z = pc.read(); if (z == -1) delayMicroseconds(10); } while (z < 0);
260   }
261   return num;
262 }
263 
264 void nacitaj_choreografiu()
265 {
266   ch_len = 0;
267   int tm;
268   do { 
269     tm = nacitajCislo();
270     ch_time[ch_len] = tm;
271     ch_servo[ch_len] = nacitajCislo();
272     ch_val[ch_len] = nacitajCislo();
273     ch_len++;
274   if (ch_len == CHOREO_LEN) break;
275   } while (tm > 0);
276   pipni();  
277 }
278 
279 void vypis_choreografiu()
280 {
281   for (int i = 0; i < ch_len; i++)
282   {
283     pc.print(ch_time[i]);
284     pc.print(" ");
285     pc.print(ch_servo[i]);
286     pc.print(" ");
287     pc.println(ch_val[i]);      
288   }
289   pipni();
290 }
291 
292 void zatancuj_choreografiu()
293 {
294   for (int i = 0; i < ch_len; i++)
295   {
296     delay(ch_time[i]);
297     nastav_koncatinu(ch_servo[i], ch_val[i]);
298   }
299   if (ch_len > 0) stav[ch_servo[ch_len - 1]] = ch_val[ch_len - 1];
300   pipni();
301 }
302 
303 void reset()
304 {
305   for (int i = 0; i < 6; i++) 
306   {
307     stav[i] = kalib[i];
308     s[i].write(kalib[i]);
309   }
310   pipni();
311 }
312 
313 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
314 {
315     pc.print(" (+/-/ENTER): ");
316     pc.println(hodnota);
317     s[servo].write(hodnota);
318     char z;
319     do {
320       z = pc.read();
321       if ((z == '+') && (hodnota < 180)) hodnota++;
322       else if ((z == '-') && (hodnota > 0)) hodnota--; 
323       if ((z == '+') || (z == '-'))
324       {
325         pc.print(hodnota); pc.print('\r'); 
326         s[servo].write(hodnota);
327       }
328     } while (z != 13);
329     return hodnota;
330 }
331 
332 void kalibruj()
333 {
334   for (int i = 0; i < 6; i++)
335   {
336     pc.print(i);
337     kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
338     pc.print(i);
339     pc.print(": ");
340     pc.println(kalib[i]);
341   }
342   for (int i = 0; i < 6; i++) { pc.print(kalib[i]); pc.print(" "); }
343   pc.println("ok");
344   pipni();
345 }
346 
347 void nastav_limity()
348 {
349   for (int i = 0; i < 6; i++)
350   {
351     pc.print(i);
352     pc.print("dolny");
353     dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
354     pc.print(i);
355     pc.print(" dolny: ");
356     pc.println(dolny_limit[i]);
357     s[i].write(kalib[i]);
358 
359     pc.print(i);
360     pc.print("horny");
361     horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
362     pc.print(i);
363     pc.print(" horny: ");
364     pc.println(horny_limit[i]);
365     s[i].write(kalib[i]);
366   }
367   for (int i = 0; i < 6; i++) { pc.print(dolny_limit[i]); pc.print("-"); pc.print(horny_limit[i]); pc.print(" "); }
368   pc.println("ok");
369   pipni();
370 }
371 
372 void vypis_kalibraciu()
373 {
374   pc.print("stredy: ");
375   for (int i = 0; i < 6; i++) { pc.print(kalib[i]); pc.print(" "); }
376   pc.println();
377   pc.print("dolny limit: ");
378   for (int i = 0; i < 6; i++) { pc.print(dolny_limit[i]); pc.print(" "); }
379   pc.println();
380   pc.print("horny limit: ");
381   for (int i = 0; i < 6; i++) { pc.print(horny_limit[i]); pc.print(" "); }
382   pc.println();
383 }
384 
385 void nacitaj_kalibraciu()
386 {
387   int tm;
388   for (int i = 0; i < 6; i++) 
389     kalib[i] = nacitajCislo();
390   vypis_kalibraciu();
391   pc.println("ok");
392   pipni();
393 }
394 
395 void zvys_krok()
396 {
397   if (krok < 180) krok++;
398   pc.print("krok: ");
399   pc.println(krok);
400 }
401 
402 void zniz_krok()
403 {
404   if (krok > 0) krok--;
405   pc.print("krok: ");
406   pc.println(krok);
407 }
408 
409 void loop() {
410   if (pc.available())
411   { 
412     char z = pc.read();
413     if (pohyb_znakom(z)) return;   
414     else if (pohyb_kombinacia(z)) return; 
415     else if (z == '@') nacitaj_choreografiu();
416     else if (z == '?') vypis_choreografiu();
417     else if (z == 't') zatancuj_choreografiu();
418     else if (z == '-') ahoj();
419     else if (z == ' ') reset();
420     else if (z == 'H') kalibruj();
421     else if (z == 'J') nastav_limity();
422     else if (z == 'G') vypis_kalibraciu();
423     else if (z == 'L') nacitaj_kalibraciu();
424     else if (z == 'E') zapis_kalibraciu_do_EEPROM();
425     else if (z == 'R') ruky();
426     else if (z == '9') zvys_krok();
427     else if (z == '1') zniz_krok();
428   }
429 }