Otto - riadiaci program v.1

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