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

From DT^2
Jump to: navigation, search
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
 
''Pre komunikáciu na pinoch 0,1 pomocou bežného sériového portu treba pridať riadok

Revision as of 21: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 }