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

From DT^2
Jump to: navigation, search
m
m
Line 135: Line 135:
 
   Serial.begin(9600);
 
   Serial.begin(9600);
 
   ch_len = 0;
 
   ch_len = 0;
   krok = 1;
+
   krok = 7;
 
   ahoj();
 
   ahoj();
 
   ruky();
 
   ruky();

Revision as of 06:07, 2 August 2018

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