Otto - riadiaci program v.1

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