Otto - riadiaci program v.1

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

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ť riadky pc.begin(...); a SoftwareSerial pc(...)

  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 #define SIRENA 12
 11 
 12 //maximalna dlzka choreografie
 13 #define CHOREO_LEN 200
 14 
 15 // v subore s choreografiou su serva cislovane takto:
 16 //
 17 // 1 - SERVO_PRAVA_PATA
 18 // 2 - SERVO_LAVA_PATA
 19 // 3 - SERVO_PRAVA_NOHA
 20 // 4 - SERVO_LAVA_NOHA
 21 // 5 - SERVO_PRAVA_RUKA
 22 // 6 - SERVO_LAVA_RUKA
 23 
 24 // cisla pinov, kde su zapojene servo motory
 25 #define SERVO_LAVA_RUKA   10 
 26 #define SERVO_PRAVA_RUKA  11
 27 #define SERVO_LAVA_NOHA   9
 28 #define SERVO_PRAVA_NOHA  6
 29 #define SERVO_LAVA_PATA   5
 30 #define SERVO_PRAVA_PATA  3
 31 
 32 // ak su niektore serva naopak, je tu jednotka
 33 uint8_t servo_invertovane[6] = {0, 0, 1, 1, 0, 1};
 34 
 35 // znaky, ktorymi sa ovladaju jednotlive stupne volnosti
 36 char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' };
 37 // co robia jednotlive znaky (znamienko urcuje smer)
 38 int8_t zmeny[] = {-SERVO_LAVA_RUKA, SERVO_LAVA_RUKA, 
 39                   -SERVO_PRAVA_RUKA, SERVO_PRAVA_RUKA, 
 40                   -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA, 
 41                   -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA, 
 42                    SERVO_LAVA_PATA, -SERVO_LAVA_PATA, 
 43                    SERVO_PRAVA_PATA, -SERVO_PRAVA_PATA }; 
 44 
 45 // sem si mozno ulozit svoju kalibraciu
 46 uint8_t prednastavena_kalibracia[] = { 90, 90, 90, 90, 90, 90 };
 47 
 48 Servo s[6];
 49 SoftwareSerial pc(BT_TX, BT_RX);
 50 
 51 uint16_t ch_time[CHOREO_LEN];
 52 uint8_t ch_servo[CHOREO_LEN];
 53 uint8_t ch_val[CHOREO_LEN];
 54 int ch_len;
 55 uint8_t kalib[6];
 56 int stav[6];
 57 int krok;
 58 
 59 void setup() {
 60   randomSeed(analogRead(1));
 61   s[0].attach(SERVO_PRAVA_PATA);
 62   s[1].attach(SERVO_LAVA_PATA);
 63   s[2].attach(SERVO_PRAVA_NOHA);
 64   s[3].attach(SERVO_LAVA_NOHA);
 65   s[4].attach(SERVO_PRAVA_RUKA);
 66   s[5].attach(SERVO_LAVA_RUKA);
 67   for (int i = 0; i < 6; i++)
 68   {
 69     kalib[i] = prednastavena_kalibracia[i];
 70     stav[i] = kalib[i];
 71     s[i].write(stav[i]);
 72   }
 73   pc.begin(9600);
 74   Serial.begin(9600);
 75   ch_len = 0;
 76   krok = 1;
 77   ahoj();
 78   ruky();
 79 }
 80 
 81 void pipni()
 82 {
 83   tone(SIRENA, 1568, 50);
 84   delay(100);
 85   tone(SIRENA, 1357, 50);
 86 }
 87 
 88 void ruky()
 89 {
 90   int odloz_krok = krok;
 91   delay(500);
 92   krok = 90;
 93   pohyb(SERVO_LAVA_RUKA);    
 94   pohyb(SERVO_PRAVA_RUKA);
 95   delay(1000);
 96   krok = 180;
 97   pohyb(-SERVO_LAVA_RUKA);    
 98   pohyb(-SERVO_PRAVA_RUKA);
 99   delay(1000);
100   krok = odloz_krok;
101   pipni();
102 }
103 
104 void ahoj()
105 {
106   tone(SIRENA, 1568, 50);
107   delay(70);
108   tone(SIRENA, 1175, 30);
109   delay(50);
110   tone(SIRENA, 880, 30);
111   delay(50);
112   tone(SIRENA, 1047, 50);
113   delay(70);
114   tone(SIRENA, 1245, 30);
115   delay(150);
116   tone(SIRENA, 1568, 50);
117   delay(100);
118   if (random(10) > 4) tone(SIRENA, 1357, 50);
119   else tone(SIRENA, 1047, 50);
120 }
121 
122 void pohyb(int8_t servo)
123 {
124   int8_t srv = (servo > 0)?servo:-servo;
125   srv--;
126   if (servo_invertovane[srv]) servo = -servo;
127   if (servo > 0)
128   {
129     if (stav[srv] <= 180 - krok) stav[srv] += krok;
130     else stav[srv] = 180;
131     s[srv].write(stav[srv]);
132   }
133   else if (servo < 0)
134   {
135     if (stav[srv] >= krok) stav[srv] -= krok; 
136     else stav[srv] = 0;
137     s[srv].write(stav[srv]);      
138   }
139 }
140 
141 uint8_t pohyb_znakom(char z)
142 {
143   for (int i = 0; i < 12; i++)
144   {
145     if (z == znaky_zmien[i])
146     {
147       int8_t servo = zmeny[i];
148       pohyb(servo);
149     }
150   }
151 }
152 
153 void kombinacia1()
154 {
155   pohyb(SERVO_LAVA_NOHA);
156   pohyb(-SERVO_PRAVA_PATA);
157 }
158 
159 void kombinacia2()
160 {
161   pohyb(SERVO_PRAVA_NOHA);
162   pohyb(-SERVO_LAVA_PATA);
163 }
164 
165 void kombinacia3()
166 {
167    pohyb(SERVO_LAVA_RUKA); 
168    pohyb(SERVO_PRAVA_RUKA); 
169 }
170 
171 void kombinacia4()
172 {
173    pohyb(-SERVO_LAVA_RUKA); 
174    pohyb(-SERVO_PRAVA_RUKA); 
175 }
176 
177 int pohyb_kombinacia(char z)
178 {
179   if (z == '3') kombinacia1();
180   else if (z == '4') kombinacia2();
181   else if (z == '5') kombinacia3();
182   else if (z == '6') kombinacia4();
183   else return 0;
184   return 1;
185 }
186 
187 int nacitajCislo()
188 {
189   int num = 0;
190   int z;
191   do {
192     z = pc.read();
193     if (z == '#') while (z != 13) z = pc.read();
194   } while ((z < '0') || (z > '9'));
195   while ((z >= '0') && (z <= '9'))
196   {
197     num *= 10;
198     num += (z - '0');
199     do { z = pc.read(); if (z == -1) delayMicroseconds(10); } while (z < 0);
200   }
201   return num;
202 }
203 
204 void nacitaj_choreografiu()
205 {
206   ch_len = 0;
207   int tm;
208   do { 
209     tm = nacitajCislo();
210     ch_time[ch_len] = tm;
211     ch_servo[ch_len] = nacitajCislo();
212     ch_val[ch_len] = nacitajCislo();
213     ch_len++;
214   if (ch_len == CHOREO_LEN) break;
215   } while (tm > 0);
216   pipni();  
217 }
218 
219 void vypis_choreografiu()
220 {
221   for (int i = 0; i < ch_len; i++)
222   {
223     pc.print(ch_time[i]);
224     pc.print(" ");
225     pc.print(ch_servo[i]);
226     pc.print(" ");
227     pc.println(ch_val[i]);      
228   }
229   pipni();
230 }
231 
232 void zatancuj_choreografiu()
233 {
234   for (int i = 0; i < ch_len; i++)
235   {
236     delay(ch_time[i]);
237     s[ch_servo[i] - 1].write(ch_val[i]);
238   }
239   pipni();
240 }
241 
242 void reset()
243 {
244   for (int i = 0; i < 6; i++) 
245   {
246     stav[i] = kalib[i];
247     s[i].write(kalib[i]);
248   }
249   pipni();
250 }
251 
252 void kalibruj()
253 {
254   for (int i = 0; i < 6; i++)
255   {
256     pc.print(i);
257     pc.print(" (+/-/ENTER): ");
258     pc.println(kalib[i]);
259     s[i].write(kalib[i]);
260     char z;
261     do {
262       z = pc.read();
263       if ((z == '+') && (kalib[i] < 180)) { kalib[i]++; pc.print(kalib[i]); pc.print('\r'); s[i].write(kalib[i]); }
264       else if ((z == '-') && (kalib[i] > 0)) { kalib[i]--; pc.print(kalib[i]); pc.print('\r'); s[i].write(kalib[i]); }
265     } while (z != 13);
266     pc.print(i);
267     pc.print(": ");
268     pc.println(kalib[i]);
269   }
270   for (int i = 0; i < 6; i++) { pc.print(kalib[i]); pc.print(" "); }
271   pc.println("ok");
272   pipni();
273 }
274 
275 void nacitaj_kalibraciu()
276 {
277   ch_len = 0;
278   int tm;
279   for (int i = 0; i < 6; i++) 
280     kalib[i] = nacitajCislo();
281   for (int i = 0; i < 6; i++) { pc.print(kalib[i]); pc.print(" "); }
282   pc.println("ok");
283   pipni();
284 }
285 
286 void zvys_krok()
287 {
288   if (krok < 180) krok++;
289   pc.print("krok: ");
290   pc.println(krok);
291 }
292 
293 void zniz_krok()
294 {
295   if (krok > 0) krok--;
296   pc.print("krok: ");
297   pc.println(krok);
298 }
299 
300 void loop() {
301   if (pc.available())
302   { 
303     char z = pc.read();
304     if (pohyb_znakom(z)) return;   
305     else if (pohyb_kombinacia(z)) return; 
306     else if (z == '@') nacitaj_choreografiu();
307     else if (z == '?') vypis_choreografiu();
308     else if (z == 't') zatancuj_choreografiu();
309     else if (z == '-') ahoj();
310     else if (z == ' ') reset();
311     else if (z == 'H') kalibruj();
312     else if (z == 'L') nacitaj_kalibraciu();
313     else if (z == '9') zvys_krok();
314     else if (z == '1') zniz_krok();
315   }
316 }