Difference between revisions of "Otto - riadiaci program v.1"

Revision as of 19:09, 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 }

@@ Line 27: / Line 27: @@
 |}
-Ďalej umožňuje kalibrovať strednú polohu servomotorov (pomocou klávesu H),
+Ďalej umožňuje
-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.
+*kalibrovať strednú polohu servomotorov (pomocou klávesu '''H'''),
-Na klávesách 3-6 je možné nastaviť rôznu kombináciu (funkcie kombinacia1() - kombinacia4()).
+*pozdraví po klávese '''-''',
-Medzera resetne všetky servo motory do strednej polohy.
+*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

Difference between revisions of "Otto - riadiaci program v.1"

Revision as of 19:09, 1 August 2018

Navigation menu

Personal tools

Namespaces

Variants

Views

More

Search

Navigation