Otto Robot

Otto is an opensource, Arduino-compatible, 3D-printable two-legged robot. It can be programmed from any computer to walk, dance, and sing.

Nov 30, 2017

•

5251 views

•

1 respects

servo

robots

Components and supplies

Arduino Nano R3

arduino nano sheild

Continuous Rotation Servo

Buzzer

2AA Battery Holder for RTC

Tools and machines

3D Printer (generic)

Apps and platforms

Arduino IDE

Code

otto robot

arduino

install arduino app

1//-----------------------------------------------------------------
2#include   <Servo.h>
3#include <Oscillator.h>
4#include <EEPROM.h>
5
6#define N_SERVOS   4
7//-- First step: Configure the pins where the servos are attached
8/*
9          --------------- 
10        |     O   O     |
11        |---------------|
12YR   3==> |               | <== YL 2
13         --------------- 
14            ||     ||
15             ||     ||
16RR 5==>   -----   ------  <== RL 4
17         |-----   ------|
18*/
19#define   EEPROM_TRIM false 
20// Activate to take callibration data from internal memory
21#define   TRIM_RR 7
22#define TRIM_RL 4
23#define TRIM_YR  4
24#define TRIM_YL -7
25//OTTO.setTrims(-7,-4,-4,7);
26
27#define   PIN_RR 5
28#define PIN_RL 4
29#define PIN_YR 3
30#define PIN_YL 2
31
32#define   INTERVALTIME 10.0 
33
34Oscillator servo[N_SERVOS];
35
36void goingUp(int tempo);
37void   drunk (int tempo);
38void noGravity(int tempo);
39void kickLeft(int tempo);
40void   kickRight(int tempo);
41void run(int steps, int T=500);
42void walk(int steps,   int T=1000);
43void backyard(int steps, int T=3000);
44void backyardSlow(int steps,   int T=5000);
45void turnLeft(int steps, int T=3000);
46void turnRight(int steps,   int T=3000);
47void moonWalkLeft(int steps, int T=1000);
48void moonWalkRight(int   steps, int T=1000);
49void crusaito(int steps, int T=1000);
50void swing(int steps,   int T=1000);
51void upDown(int steps, int T=1000);
52void flapping(int steps,   int T=1000);
53
54void setup()
55{
56  Serial.begin(19200);
57  
58  servo[0].attach(PIN_RR);
59   servo[1].attach(PIN_RL);
60  servo[2].attach(PIN_YR);
61  servo[3].attach(PIN_YL);
62   
63  //EEPROM.write(0,TRIM_RR);
64  //EEPROM.write(1,TRIM_RL);
65  //EEPROM.write(2,TRIM_YR);
66   //EEPROM.write(3,TRIM_YL);
67  
68  int trim;
69  
70  if(EEPROM_TRIM){
71     for(int x=0;x<4;x++){
72      trim=EEPROM.read(x);
73      if(trim>128)trim=trim-256;
74       Serial.print("TRIM ");
75      Serial.print(x);
76      Serial.print("   en ");
77      Serial.println(trim);
78      servo[x].SetTrim(trim);
79    }
80   }
81  else{
82    servo[0].SetTrim(TRIM_RR);
83    servo[1].SetTrim(TRIM_RL);
84     servo[2].SetTrim(TRIM_YR);
85    servo[3].SetTrim(TRIM_YL);
86  }
87  
88   for(int i=0;i<4;i++) servo[i].SetPosition(90);
89}
90
91// TEMPO: 121 BPM
92int   t=495;
93double pause=0;
94
95void loop()
96{
97 // if(Serial.available()){
98   //  char init = Serial.read();
99   // if (init=='X'){
100   //   delay(4000);   //3000 - 4500
101   
102dance();
103
104
105//for(int i=0;i<4;i++) servo[i].SetPosition(90);
106
107       
108          for(int i=0;i<4;i++) servo[i].SetPosition(90);
109   // }
110   // }
111}
112
113void dance(){
114  primera_parte();
115  segunda_parte();
116  moonWalkLeft(4,t*2);
117   moonWalkRight(4,t*2);
118  moonWalkLeft(4,t*2);
119  moonWalkRight(4,t*2);
120   primera_parte(); 
121  crusaito(1,t*8);
122  crusaito(1,t*7);
123
124  for (int   i=0; i<16; i++){
125    flapping(1,t/4);
126    delay(3*t/4);
127  }
128 
129  moonWalkRight(4,t*2);
130   moonWalkLeft(4,t*2);
131  moonWalkRight(4,t*2);
132  moonWalkLeft(4,t*2);
133
134   drunk(t*4);
135  drunk(t*4);
136  drunk(t*4);
137  drunk(t*4);
138  kickLeft(t);
139   kickRight(t);
140  drunk(t*8);
141  drunk(t*4);
142  drunk(t/2);
143  delay(t*4);   
144
145  drunk(t/2);
146
147  delay(t*4); 
148  walk(2,t*2);
149  backyard(2,t*2);
150   goingUp(t*2);
151  goingUp(t*1);
152  noGravity(t*2);
153  crusaito(1,t*2);
154   crusaito(1,t*8);
155  crusaito(1,t*2);
156  crusaito(1,t*8);
157  crusaito(1,t*2);
158   crusaito(1,t*3);
159
160  delay(t);
161  primera_parte();
162    for (int i=0;   i<32; i++){
163    flapping(1,t/2);
164    delay(t/2);
165  }
166  
167  for(int   i=0;i<4;i++) servo[i].SetPosition(90);
168}
169
170
171
172
173////////////////////////////////////////////////////////////////////////////////////////
174//////////////////////////////////FUNCIONES   DE CONTROL//////////////////////////////////
175////////////////////////////////////////////////////////////////////////////////////////
176
177void   oscillate(int A[N_SERVOS], int O[N_SERVOS], int T, double phase_diff[N_SERVOS]){
178   for (int i=0; i<4; i++) {
179    servo[i].SetO(O[i]);
180    servo[i].SetA(A[i]);
181     servo[i].SetT(T);
182    servo[i].SetPh(phase_diff[i]);
183  }
184  double ref=millis();
185    for (double x=ref; x<T+ref; x=millis()){
186     for (int i=0; i<4; i++){
187         servo[i].refresh();
188     }
189  }
190}
191
192unsigned long final_time;
193unsigned   long interval_time;
194int oneTime;
195int iteration;
196float increment[N_SERVOS];   
197int oldPosition[]={90,90,90,90};
198
199void moveNServos(int time, int  newPosition[]){
200   for(int i=0;i<N_SERVOS;i++)  increment[i] = ((newPosition[i])-oldPosition[i])/(time/INTERVALTIME);
201   
202  final_time =  millis() + time; 
203  
204  iteration = 1; 
205  while(millis()   < final_time){ //Javi del futuro cambia esto  
206      interval_time = millis()+INTERVALTIME;   
207      
208      oneTime=0;      
209      while(millis()<interval_time){    
210           if(oneTime<1){ 
211              for(int i=0;i<N_SERVOS;i++){
212                  servo[i].SetPosition(oldPosition[i]   + (iteration * increment[i]));
213              }     
214              iteration++;
215               oneTime++;
216          }
217      }     
218  }   
219
220  for(int   i=0;i<N_SERVOS;i++){  
221    oldPosition[i] = newPosition[i];
222  }   
223}
224
225
226//////////////////////////////////////////////////////////////////////////////
227////////////////////////////////PASOS   DE BAILE////////////////////////////////
228//////////////////////////////////////////////////////////////////////////////
229
230void   goingUp(int tempo){
231  
232      pause=millis();
233      for(int i=0;i<4;i++)   servo[i].SetPosition(90);
234      delay(tempo);
235      servo[0].SetPosition(80);
236       servo[1].SetPosition(100);
237      delay(tempo);
238      servo[0].SetPosition(70);
239       servo[1].SetPosition(110);
240      delay(tempo);
241      servo[0].SetPosition(60);
242       servo[1].SetPosition(120);
243      delay(tempo);
244      servo[0].SetPosition(50);
245       servo[1].SetPosition(130);
246      delay(tempo);
247      servo[0].SetPosition(40);
248       servo[1].SetPosition(140);
249      delay(tempo);
250      servo[0].SetPosition(30);
251       servo[1].SetPosition(150);
252      delay(tempo);
253      servo[0].SetPosition(20);
254       servo[1].SetPosition(160);
255      delay(tempo);
256      
257      while(millis()<pause+8*t);
258
259}
260
261void   primera_parte(){
262  
263  int move1[4] = {60,120,90,90};
264  int move2[4] = {90,90,90,90};
265   int move3[4] = {40,140,90,90};
266  
267  for(int x=0; x<3; x++){
268    for(int   i=0; i<3; i++){
269      lateral_fuerte(1,t/2);
270      lateral_fuerte(0,t/4);
271       lateral_fuerte(1,t/4);
272      delay(t);
273    }
274  
275    pause=millis();
276     for(int i=0;i<4;i++) servo[i].SetPosition(90);
277    moveNServos(t*0.4,move1);
278     moveNServos(t*0.4,move2);
279    while(millis()<(pause+t*2));
280  }
281  
282   for(int i=0; i<2; i++){
283    lateral_fuerte(1,t/2);
284    lateral_fuerte(0,t/4);
285     lateral_fuerte(1,t/4);
286    delay(t);
287  }
288    
289  pause=millis();
290   for(int i=0;i<4;i++) servo[i].SetPosition(90);
291  crusaito(1,t*1.4);
292  moveNServos(t*1,move3);
293   for(int i=0;i<4;i++) servo[i].SetPosition(90);
294  while(millis()<(pause+t*4));
295}
296
297void   segunda_parte(){
298  
299  int move1[4] = {90,90,80,100};
300  int move2[4] = {90,90,100,80};
301   int move3[4] = {90,90,80,100};
302  int move4[4] = {90,90,100,80};
303    
304   int move5[4] = {40,140,80,100};
305  int move6[4] = {40,140,100,80};
306  int   move7[4] = {90,90,80,100};
307  int move8[4] = {90,90,100,80};
308       
309  int   move9[4] = {40,140,80,100};
310  int move10[4] = {40,140,100,80};
311  int move11[4]   = {90,90,80,100};
312  int move12[4] = {90,90,100,80};
313  
314  for(int x=0; x<7;   x++){ 
315    for(int i=0; i<3; i++){
316      pause=millis();
317      moveNServos(t*0.15,move1);
318       moveNServos(t*0.15,move2);
319      moveNServos(t*0.15,move3);
320      moveNServos(t*0.15,move4);
321       while(millis()<(pause+t));
322    }
323    pause=millis();
324    moveNServos(t*0.15,move5);
325     moveNServos(t*0.15,move6);
326    moveNServos(t*0.15,move7);
327    moveNServos(t*0.15,move8);
328     while(millis()<(pause+t));
329  }
330 
331  for(int i=0; i<3; i++){
332    pause=millis();
333     moveNServos(t*0.15,move9);
334    moveNServos(t*0.15,move10);
335    moveNServos(t*0.15,move11);
336     moveNServos(t*0.15,move12);
337    while(millis()<(pause+t));
338  }
339}
340
341void   lateral_fuerte(boolean side, int tempo){
342  
343  for(int i=0;i<4;i++) servo[i].SetPosition(90);
344   if (side) servo[0].SetPosition(40);
345  else servo[1].SetPosition(140);
346  delay(tempo/2);
347   servo[0].SetPosition(90);
348  servo[1].SetPosition(90);
349  delay(tempo/2);
350
351}
352
353void   drunk (int tempo){
354  
355  pause=millis();
356  
357  int move1[] = {60,70,90,90};
358   int move2[] = {110,120,90,90};
359  int move3[] = {60,70,90,90};
360  int move4[]   = {110,120,90,90};
361  
362  moveNServos(tempo*0.235,move1);
363  moveNServos(tempo*0.235,move2);
364   moveNServos(tempo*0.235,move3);
365  moveNServos(tempo*0.235,move4);
366  while(millis()<(pause+tempo));
367
368}
369
370
371void   noGravity(int tempo){
372  
373  int move1[4] = {120,140,90,90};
374  int move2[4]   = {140,140,90,90};
375  int move3[4] = {120,140,90,90};
376  int move4[4] = {90,90,90,90};
377   
378  
379  for(int i=0;i<4;i++) servo[i].SetPosition(90);
380  for(int i=0;i<N_SERVOS;i++)   oldPosition[i]=90;
381  moveNServos(tempo*2,move1);
382  moveNServos(tempo*2,move2);
383   delay(tempo*2);
384  moveNServos(tempo*2,move3);
385  moveNServos(tempo*2,move4);
386
387   
388
389}
390
391void kickLeft(int tempo){
392  for(int i=0;i<4;i++) servo[i].SetPosition(90);
393   delay(tempo);
394  servo[0].SetPosition(50); //pie derecho
395  servo[1].SetPosition(70);   //pie izquiero
396  delay(tempo);
397  servo[0].SetPosition(80); //pie derecho
398   servo[1].SetPosition(70); //pie izquiero
399  delay(tempo/4);
400  servo[0].SetPosition(30);   //pie derecho
401  servo[1].SetPosition(70); //pie izquiero
402  delay(tempo/4);
403   servo[0].SetPosition(80); //pie derecho
404  servo[1].SetPosition(70); //pie izquiero
405   delay(tempo/4);
406  servo[0].SetPosition(30); //pie derecho
407  servo[1].SetPosition(70);   //pie izquiero
408  delay(tempo/4);
409  servo[0].SetPosition(80); //pie derecho
410   servo[1].SetPosition(70); //pie izquiero
411  delay(tempo);
412}
413
414void kickRight(int   tempo){
415for(int i=0;i<4;i++) servo[i].SetPosition(90);
416  delay(tempo);
417   servo[0].SetPosition(110); //pie derecho
418  servo[1].SetPosition(130); //pie   izquiero
419  delay(tempo);
420  servo[0].SetPosition(110); //pie derecho
421  servo[1].SetPosition(100);   //pie izquiero
422  delay(tempo/4);
423  servo[0].SetPosition(110); //pie derecho
424   servo[1].SetPosition(150); //pie izquiero
425  delay(tempo/4);
426  servo[0].SetPosition(110);   //pie derecho
427  servo[1].SetPosition(80); //pie izquiero
428  delay(tempo/4);
429   servo[0].SetPosition(110); //pie derecho
430  servo[1].SetPosition(150); //pie   izquiero
431  delay(tempo/4);
432  servo[0].SetPosition(110); //pie derecho
433   servo[1].SetPosition(100); //pie izquiero
434  delay(tempo);
435}
436
437void   walk(int steps, int T){
438    int A[4]= {15, 15, 30, 30};
439    int O[4] = {0,   0, 0, 0};
440    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};
441     
442    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
443}
444
445void   run(int steps, int T){
446    int A[4]= {10, 10, 10, 10};
447    int O[4] = {0,   0, 0, 0};
448    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};   
449    
450    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
451}
452
453void   backyard(int steps, int T){
454    int A[4]= {15, 15, 30, 30};
455    int O[4] =   {0, 0, 0, 0};
456    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(-90),   DEG2RAD(-90)}; 
457    
458    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
459}
460
461void   backyardSlow(int steps, int T){
462    int A[4]= {15, 15, 30, 30};
463    int O[4]   = {0, 0, 0, 0};
464    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(-90),   DEG2RAD(-90)}; 
465    
466    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
467}
468
469
470void   turnLeft(int steps, int T){
471    int A[4]= {20, 20, 10, 30};
472    int O[4] =   {0, 0, 0, 0};
473    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90),   DEG2RAD(90)}; 
474    
475    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
476}
477
478void   turnRight(int steps, int T){
479    int A[4]= {20, 20, 30, 10};
480    int O[4]   = {0, 0, 0, 0};
481    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90),   DEG2RAD(90)}; 
482    
483    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
484}
485
486void   moonWalkRight(int steps, int T){
487    int A[4]= {25, 25, 0, 0};
488    int O[4]   = {-15 ,15, 0, 0};
489    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180 + 120),   DEG2RAD(90), DEG2RAD(90)}; 
490    
491    for(int i=0;i<steps;i++)oscillate(A,O,   T, phase_diff);
492}
493
494void moonWalkLeft(int steps, int T){
495    int A[4]=   {25, 25, 0, 0};
496    int O[4] = {-15, 15, 0, 0};
497    double phase_diff[4] =   {DEG2RAD(0), DEG2RAD(180 - 120), DEG2RAD(90), DEG2RAD(90)}; 
498    
499    for(int   i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
500}
501
502void crusaito(int steps,   int T){
503    int A[4]= {25, 25, 30, 30};
504    int O[4] = {- 15, 15, 0, 0};
505     double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180 + 120), DEG2RAD(90), DEG2RAD(90)};   
506    
507    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
508}
509
510void   swing(int steps, int T){
511    int A[4]= {25, 25, 0, 0};
512    int O[4] = {-15,   15, 0, 0};
513    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};
514     
515    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
516}
517
518void   upDown(int steps, int T){
519    int A[4]= {25, 25, 0, 0};
520    int O[4] = {-15,   15, 0, 0};
521    double phase_diff[4] = {DEG2RAD(180), DEG2RAD(0), DEG2RAD(270),   DEG2RAD(270)};
522    
523    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
524}
525
526void   flapping(int steps, int T){
527    int A[4]= {15, 15, 8, 8};
528    int O[4] = {-A[0],   A[1], 0, 0};
529    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180), DEG2RAD(90),   DEG2RAD(-90)};
530    
531    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
532}
533
534void   test(int steps, int T){
535    int A[4]= {15, 15, 8, 8};
536    int O[4] = {-A[0]   + 10, A[1] - 10, 0, 0};
537    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180),   DEG2RAD(90), DEG2RAD(-90)};
538    
539    for(int i=0;i<steps;i++)oscillate(A,O,   T, phase_diff);
540}
541
542
543

//-----------------------------------------------------------------
#include   <Servo.h>
#include <Oscillator.h>
#include <EEPROM.h>

#define N_SERVOS   4
//-- First step: Configure the pins where the servos are attached
/*
          --------------- 
        |     O   O     |
        |---------------|
YR   3==> |               | <== YL 2
         --------------- 
            ||     ||
             ||     ||
RR 5==>   -----   ------  <== RL 4
         |-----   ------|
*/
#define   EEPROM_TRIM false 
// Activate to take callibration data from internal memory
#define   TRIM_RR 7
#define TRIM_RL 4
#define TRIM_YR  4
#define TRIM_YL -7
//OTTO.setTrims(-7,-4,-4,7);

#define   PIN_RR 5
#define PIN_RL 4
#define PIN_YR 3
#define PIN_YL 2

#define   INTERVALTIME 10.0 

Oscillator servo[N_SERVOS];

void goingUp(int tempo);
void   drunk (int tempo);
void noGravity(int tempo);
void kickLeft(int tempo);
void   kickRight(int tempo);
void run(int steps, int T=500);
void walk(int steps,   int T=1000);
void backyard(int steps, int T=3000);
void backyardSlow(int steps,   int T=5000);
void turnLeft(int steps, int T=3000);
void turnRight(int steps,   int T=3000);
void moonWalkLeft(int steps, int T=1000);
void moonWalkRight(int   steps, int T=1000);
void crusaito(int steps, int T=1000);
void swing(int steps,   int T=1000);
void upDown(int steps, int T=1000);
void flapping(int steps,   int T=1000);

void setup()
{
  Serial.begin(19200);
  
  servo[0].attach(PIN_RR);
   servo[1].attach(PIN_RL);
  servo[2].attach(PIN_YR);
  servo[3].attach(PIN_YL);
   
  //EEPROM.write(0,TRIM_RR);
  //EEPROM.write(1,TRIM_RL);
  //EEPROM.write(2,TRIM_YR);
   //EEPROM.write(3,TRIM_YL);
  
  int trim;
  
  if(EEPROM_TRIM){
     for(int x=0;x<4;x++){
      trim=EEPROM.read(x);
      if(trim>128)trim=trim-256;
       Serial.print("TRIM ");
      Serial.print(x);
      Serial.print("   en ");
      Serial.println(trim);
      servo[x].SetTrim(trim);
    }
   }
  else{
    servo[0].SetTrim(TRIM_RR);
    servo[1].SetTrim(TRIM_RL);
     servo[2].SetTrim(TRIM_YR);
    servo[3].SetTrim(TRIM_YL);
  }
  
   for(int i=0;i<4;i++) servo[i].SetPosition(90);
}

// TEMPO: 121 BPM
int   t=495;
double pause=0;

void loop()
{
 // if(Serial.available()){
   //  char init = Serial.read();
   // if (init=='X'){
   //   delay(4000);   //3000 - 4500
   
dance();


//for(int i=0;i<4;i++) servo[i].SetPosition(90);

       
          for(int i=0;i<4;i++) servo[i].SetPosition(90);
   // }
   // }
}

void dance(){
  primera_parte();
  segunda_parte();
  moonWalkLeft(4,t*2);
   moonWalkRight(4,t*2);
  moonWalkLeft(4,t*2);
  moonWalkRight(4,t*2);
   primera_parte(); 
  crusaito(1,t*8);
  crusaito(1,t*7);

  for (int   i=0; i<16; i++){
    flapping(1,t/4);
    delay(3*t/4);
  }
 
  moonWalkRight(4,t*2);
   moonWalkLeft(4,t*2);
  moonWalkRight(4,t*2);
  moonWalkLeft(4,t*2);

   drunk(t*4);
  drunk(t*4);
  drunk(t*4);
  drunk(t*4);
  kickLeft(t);
   kickRight(t);
  drunk(t*8);
  drunk(t*4);
  drunk(t/2);
  delay(t*4);   

  drunk(t/2);

  delay(t*4); 
  walk(2,t*2);
  backyard(2,t*2);
   goingUp(t*2);
  goingUp(t*1);
  noGravity(t*2);
  crusaito(1,t*2);
   crusaito(1,t*8);
  crusaito(1,t*2);
  crusaito(1,t*8);
  crusaito(1,t*2);
   crusaito(1,t*3);

  delay(t);
  primera_parte();
    for (int i=0;   i<32; i++){
    flapping(1,t/2);
    delay(t/2);
  }
  
  for(int   i=0;i<4;i++) servo[i].SetPosition(90);
}




////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////FUNCIONES   DE CONTROL//////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

void   oscillate(int A[N_SERVOS], int O[N_SERVOS], int T, double phase_diff[N_SERVOS]){
   for (int i=0; i<4; i++) {
    servo[i].SetO(O[i]);
    servo[i].SetA(A[i]);
     servo[i].SetT(T);
    servo[i].SetPh(phase_diff[i]);
  }
  double ref=millis();
    for (double x=ref; x<T+ref; x=millis()){
     for (int i=0; i<4; i++){
         servo[i].refresh();
     }
  }
}

unsigned long final_time;
unsigned   long interval_time;
int oneTime;
int iteration;
float increment[N_SERVOS];   
int oldPosition[]={90,90,90,90};

void moveNServos(int time, int  newPosition[]){
   for(int i=0;i<N_SERVOS;i++)  increment[i] = ((newPosition[i])-oldPosition[i])/(time/INTERVALTIME);
   
  final_time =  millis() + time; 
  
  iteration = 1; 
  while(millis()   < final_time){ //Javi del futuro cambia esto  
      interval_time = millis()+INTERVALTIME;   
      
      oneTime=0;      
      while(millis()<interval_time){    
           if(oneTime<1){ 
              for(int i=0;i<N_SERVOS;i++){
                  servo[i].SetPosition(oldPosition[i]   + (iteration * increment[i]));
              }     
              iteration++;
               oneTime++;
          }
      }     
  }   

  for(int   i=0;i<N_SERVOS;i++){  
    oldPosition[i] = newPosition[i];
  }   
}


//////////////////////////////////////////////////////////////////////////////
////////////////////////////////PASOS   DE BAILE////////////////////////////////
//////////////////////////////////////////////////////////////////////////////

void   goingUp(int tempo){
  
      pause=millis();
      for(int i=0;i<4;i++)   servo[i].SetPosition(90);
      delay(tempo);
      servo[0].SetPosition(80);
       servo[1].SetPosition(100);
      delay(tempo);
      servo[0].SetPosition(70);
       servo[1].SetPosition(110);
      delay(tempo);
      servo[0].SetPosition(60);
       servo[1].SetPosition(120);
      delay(tempo);
      servo[0].SetPosition(50);
       servo[1].SetPosition(130);
      delay(tempo);
      servo[0].SetPosition(40);
       servo[1].SetPosition(140);
      delay(tempo);
      servo[0].SetPosition(30);
       servo[1].SetPosition(150);
      delay(tempo);
      servo[0].SetPosition(20);
       servo[1].SetPosition(160);
      delay(tempo);
      
      while(millis()<pause+8*t);

}

void   primera_parte(){
  
  int move1[4] = {60,120,90,90};
  int move2[4] = {90,90,90,90};
   int move3[4] = {40,140,90,90};
  
  for(int x=0; x<3; x++){
    for(int   i=0; i<3; i++){
      lateral_fuerte(1,t/2);
      lateral_fuerte(0,t/4);
       lateral_fuerte(1,t/4);
      delay(t);
    }
  
    pause=millis();
     for(int i=0;i<4;i++) servo[i].SetPosition(90);
    moveNServos(t*0.4,move1);
     moveNServos(t*0.4,move2);
    while(millis()<(pause+t*2));
  }
  
   for(int i=0; i<2; i++){
    lateral_fuerte(1,t/2);
    lateral_fuerte(0,t/4);
     lateral_fuerte(1,t/4);
    delay(t);
  }
    
  pause=millis();
   for(int i=0;i<4;i++) servo[i].SetPosition(90);
  crusaito(1,t*1.4);
  moveNServos(t*1,move3);
   for(int i=0;i<4;i++) servo[i].SetPosition(90);
  while(millis()<(pause+t*4));
}

void   segunda_parte(){
  
  int move1[4] = {90,90,80,100};
  int move2[4] = {90,90,100,80};
   int move3[4] = {90,90,80,100};
  int move4[4] = {90,90,100,80};
    
   int move5[4] = {40,140,80,100};
  int move6[4] = {40,140,100,80};
  int   move7[4] = {90,90,80,100};
  int move8[4] = {90,90,100,80};
       
  int   move9[4] = {40,140,80,100};
  int move10[4] = {40,140,100,80};
  int move11[4]   = {90,90,80,100};
  int move12[4] = {90,90,100,80};
  
  for(int x=0; x<7;   x++){ 
    for(int i=0; i<3; i++){
      pause=millis();
      moveNServos(t*0.15,move1);
       moveNServos(t*0.15,move2);
      moveNServos(t*0.15,move3);
      moveNServos(t*0.15,move4);
       while(millis()<(pause+t));
    }
    pause=millis();
    moveNServos(t*0.15,move5);
     moveNServos(t*0.15,move6);
    moveNServos(t*0.15,move7);
    moveNServos(t*0.15,move8);
     while(millis()<(pause+t));
  }
 
  for(int i=0; i<3; i++){
    pause=millis();
     moveNServos(t*0.15,move9);
    moveNServos(t*0.15,move10);
    moveNServos(t*0.15,move11);
     moveNServos(t*0.15,move12);
    while(millis()<(pause+t));
  }
}

void   lateral_fuerte(boolean side, int tempo){
  
  for(int i=0;i<4;i++) servo[i].SetPosition(90);
   if (side) servo[0].SetPosition(40);
  else servo[1].SetPosition(140);
  delay(tempo/2);
   servo[0].SetPosition(90);
  servo[1].SetPosition(90);
  delay(tempo/2);

}

void   drunk (int tempo){
  
  pause=millis();
  
  int move1[] = {60,70,90,90};
   int move2[] = {110,120,90,90};
  int move3[] = {60,70,90,90};
  int move4[]   = {110,120,90,90};
  
  moveNServos(tempo*0.235,move1);
  moveNServos(tempo*0.235,move2);
   moveNServos(tempo*0.235,move3);
  moveNServos(tempo*0.235,move4);
  while(millis()<(pause+tempo));

}


void   noGravity(int tempo){
  
  int move1[4] = {120,140,90,90};
  int move2[4]   = {140,140,90,90};
  int move3[4] = {120,140,90,90};
  int move4[4] = {90,90,90,90};
   
  
  for(int i=0;i<4;i++) servo[i].SetPosition(90);
  for(int i=0;i<N_SERVOS;i++)   oldPosition[i]=90;
  moveNServos(tempo*2,move1);
  moveNServos(tempo*2,move2);
   delay(tempo*2);
  moveNServos(tempo*2,move3);
  moveNServos(tempo*2,move4);

   

}

void kickLeft(int tempo){
  for(int i=0;i<4;i++) servo[i].SetPosition(90);
   delay(tempo);
  servo[0].SetPosition(50); //pie derecho
  servo[1].SetPosition(70);   //pie izquiero
  delay(tempo);
  servo[0].SetPosition(80); //pie derecho
   servo[1].SetPosition(70); //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(30);   //pie derecho
  servo[1].SetPosition(70); //pie izquiero
  delay(tempo/4);
   servo[0].SetPosition(80); //pie derecho
  servo[1].SetPosition(70); //pie izquiero
   delay(tempo/4);
  servo[0].SetPosition(30); //pie derecho
  servo[1].SetPosition(70);   //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(80); //pie derecho
   servo[1].SetPosition(70); //pie izquiero
  delay(tempo);
}

void kickRight(int   tempo){
for(int i=0;i<4;i++) servo[i].SetPosition(90);
  delay(tempo);
   servo[0].SetPosition(110); //pie derecho
  servo[1].SetPosition(130); //pie   izquiero
  delay(tempo);
  servo[0].SetPosition(110); //pie derecho
  servo[1].SetPosition(100);   //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(110); //pie derecho
   servo[1].SetPosition(150); //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(110);   //pie derecho
  servo[1].SetPosition(80); //pie izquiero
  delay(tempo/4);
   servo[0].SetPosition(110); //pie derecho
  servo[1].SetPosition(150); //pie   izquiero
  delay(tempo/4);
  servo[0].SetPosition(110); //pie derecho
   servo[1].SetPosition(100); //pie izquiero
  delay(tempo);
}

void   walk(int steps, int T){
    int A[4]= {15, 15, 30, 30};
    int O[4] = {0,   0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};
     
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void   run(int steps, int T){
    int A[4]= {10, 10, 10, 10};
    int O[4] = {0,   0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};   
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void   backyard(int steps, int T){
    int A[4]= {15, 15, 30, 30};
    int O[4] =   {0, 0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(-90),   DEG2RAD(-90)}; 
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void   backyardSlow(int steps, int T){
    int A[4]= {15, 15, 30, 30};
    int O[4]   = {0, 0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(-90),   DEG2RAD(-90)}; 
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}


void   turnLeft(int steps, int T){
    int A[4]= {20, 20, 10, 30};
    int O[4] =   {0, 0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90),   DEG2RAD(90)}; 
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void   turnRight(int steps, int T){
    int A[4]= {20, 20, 30, 10};
    int O[4]   = {0, 0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90),   DEG2RAD(90)}; 
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void   moonWalkRight(int steps, int T){
    int A[4]= {25, 25, 0, 0};
    int O[4]   = {-15 ,15, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180 + 120),   DEG2RAD(90), DEG2RAD(90)}; 
    
    for(int i=0;i<steps;i++)oscillate(A,O,   T, phase_diff);
}

void moonWalkLeft(int steps, int T){
    int A[4]=   {25, 25, 0, 0};
    int O[4] = {-15, 15, 0, 0};
    double phase_diff[4] =   {DEG2RAD(0), DEG2RAD(180 - 120), DEG2RAD(90), DEG2RAD(90)}; 
    
    for(int   i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void crusaito(int steps,   int T){
    int A[4]= {25, 25, 30, 30};
    int O[4] = {- 15, 15, 0, 0};
     double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180 + 120), DEG2RAD(90), DEG2RAD(90)};   
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void   swing(int steps, int T){
    int A[4]= {25, 25, 0, 0};
    int O[4] = {-15,   15, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};
     
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void   upDown(int steps, int T){
    int A[4]= {25, 25, 0, 0};
    int O[4] = {-15,   15, 0, 0};
    double phase_diff[4] = {DEG2RAD(180), DEG2RAD(0), DEG2RAD(270),   DEG2RAD(270)};
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void   flapping(int steps, int T){
    int A[4]= {15, 15, 8, 8};
    int O[4] = {-A[0],   A[1], 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180), DEG2RAD(90),   DEG2RAD(-90)};
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void   test(int steps, int T){
    int A[4]= {15, 15, 8, 8};
    int O[4] = {-A[0]   + 10, A[1] - 10, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180),   DEG2RAD(90), DEG2RAD(-90)};
    
    for(int i=0;i<steps;i++)oscillate(A,O,   T, phase_diff);
}

otto robot

arduino

install arduino app

1//-----------------------------------------------------------------
2#include  <Servo.h>
3#include <Oscillator.h>
4#include <EEPROM.h>
5
6#define N_SERVOS  4
7//-- First step: Configure the pins where the servos are attached
8/*
9         --------------- 
10        |     O   O     |
11        |---------------|
12YR  3==> |               | <== YL 2
13         --------------- 
14            ||     ||
15            ||     ||
16RR 5==>   -----   ------  <== RL 4
17         |-----   ------|
18*/
19#define  EEPROM_TRIM false 
20// Activate to take callibration data from internal memory
21#define  TRIM_RR 7
22#define TRIM_RL 4
23#define TRIM_YR  4
24#define TRIM_YL -7
25//OTTO.setTrims(-7,-4,-4,7);
26
27#define  PIN_RR 5
28#define PIN_RL 4
29#define PIN_YR 3
30#define PIN_YL 2
31
32#define  INTERVALTIME 10.0 
33
34Oscillator servo[N_SERVOS];
35
36void goingUp(int tempo);
37void  drunk (int tempo);
38void noGravity(int tempo);
39void kickLeft(int tempo);
40void  kickRight(int tempo);
41void run(int steps, int T=500);
42void walk(int steps,  int T=1000);
43void backyard(int steps, int T=3000);
44void backyardSlow(int steps,  int T=5000);
45void turnLeft(int steps, int T=3000);
46void turnRight(int steps,  int T=3000);
47void moonWalkLeft(int steps, int T=1000);
48void moonWalkRight(int  steps, int T=1000);
49void crusaito(int steps, int T=1000);
50void swing(int steps,  int T=1000);
51void upDown(int steps, int T=1000);
52void flapping(int steps,  int T=1000);
53
54void setup()
55{
56  Serial.begin(19200);
57  
58  servo[0].attach(PIN_RR);
59  servo[1].attach(PIN_RL);
60  servo[2].attach(PIN_YR);
61  servo[3].attach(PIN_YL);
62  
63  //EEPROM.write(0,TRIM_RR);
64  //EEPROM.write(1,TRIM_RL);
65  //EEPROM.write(2,TRIM_YR);
66  //EEPROM.write(3,TRIM_YL);
67  
68  int trim;
69  
70  if(EEPROM_TRIM){
71    for(int x=0;x<4;x++){
72      trim=EEPROM.read(x);
73      if(trim>128)trim=trim-256;
74      Serial.print("TRIM ");
75      Serial.print(x);
76      Serial.print("  en ");
77      Serial.println(trim);
78      servo[x].SetTrim(trim);
79    }
80  }
81  else{
82    servo[0].SetTrim(TRIM_RR);
83    servo[1].SetTrim(TRIM_RL);
84    servo[2].SetTrim(TRIM_YR);
85    servo[3].SetTrim(TRIM_YL);
86  }
87  
88  for(int i=0;i<4;i++) servo[i].SetPosition(90);
89}
90
91// TEMPO: 121 BPM
92int  t=495;
93double pause=0;
94
95void loop()
96{
97 // if(Serial.available()){
98  //  char init = Serial.read();
99   // if (init=='X'){
100   //   delay(4000);  //3000 - 4500
101   
102dance();
103
104
105//for(int i=0;i<4;i++) servo[i].SetPosition(90);
106
107      
108          for(int i=0;i<4;i++) servo[i].SetPosition(90);
109   // }
110  // }
111}
112
113void dance(){
114  primera_parte();
115  segunda_parte();
116  moonWalkLeft(4,t*2);
117  moonWalkRight(4,t*2);
118  moonWalkLeft(4,t*2);
119  moonWalkRight(4,t*2);
120  primera_parte(); 
121  crusaito(1,t*8);
122  crusaito(1,t*7);
123
124  for (int  i=0; i<16; i++){
125    flapping(1,t/4);
126    delay(3*t/4);
127  }
128 
129  moonWalkRight(4,t*2);
130  moonWalkLeft(4,t*2);
131  moonWalkRight(4,t*2);
132  moonWalkLeft(4,t*2);
133
134  drunk(t*4);
135  drunk(t*4);
136  drunk(t*4);
137  drunk(t*4);
138  kickLeft(t);
139  kickRight(t);
140  drunk(t*8);
141  drunk(t*4);
142  drunk(t/2);
143  delay(t*4);  
144
145  drunk(t/2);
146
147  delay(t*4); 
148  walk(2,t*2);
149  backyard(2,t*2);
150  goingUp(t*2);
151  goingUp(t*1);
152  noGravity(t*2);
153  crusaito(1,t*2);
154  crusaito(1,t*8);
155  crusaito(1,t*2);
156  crusaito(1,t*8);
157  crusaito(1,t*2);
158  crusaito(1,t*3);
159
160  delay(t);
161  primera_parte();
162    for (int i=0;  i<32; i++){
163    flapping(1,t/2);
164    delay(t/2);
165  }
166  
167  for(int  i=0;i<4;i++) servo[i].SetPosition(90);
168}
169
170
171
172
173////////////////////////////////////////////////////////////////////////////////////////
174//////////////////////////////////FUNCIONES  DE CONTROL//////////////////////////////////
175////////////////////////////////////////////////////////////////////////////////////////
176
177void  oscillate(int A[N_SERVOS], int O[N_SERVOS], int T, double phase_diff[N_SERVOS]){
178  for (int i=0; i<4; i++) {
179    servo[i].SetO(O[i]);
180    servo[i].SetA(A[i]);
181    servo[i].SetT(T);
182    servo[i].SetPh(phase_diff[i]);
183  }
184  double ref=millis();
185   for (double x=ref; x<T+ref; x=millis()){
186     for (int i=0; i<4; i++){
187        servo[i].refresh();
188     }
189  }
190}
191
192unsigned long final_time;
193unsigned  long interval_time;
194int oneTime;
195int iteration;
196float increment[N_SERVOS];  
197int oldPosition[]={90,90,90,90};
198
199void moveNServos(int time, int  newPosition[]){
200  for(int i=0;i<N_SERVOS;i++)  increment[i] = ((newPosition[i])-oldPosition[i])/(time/INTERVALTIME);
201  
202  final_time =  millis() + time; 
203  
204  iteration = 1; 
205  while(millis()  < final_time){ //Javi del futuro cambia esto  
206      interval_time = millis()+INTERVALTIME;  
207      
208      oneTime=0;      
209      while(millis()<interval_time){    
210          if(oneTime<1){ 
211              for(int i=0;i<N_SERVOS;i++){
212                  servo[i].SetPosition(oldPosition[i]  + (iteration * increment[i]));
213              }     
214              iteration++;
215              oneTime++;
216          }
217      }     
218  }   
219
220  for(int  i=0;i<N_SERVOS;i++){  
221    oldPosition[i] = newPosition[i];
222  }   
223}
224
225
226//////////////////////////////////////////////////////////////////////////////
227////////////////////////////////PASOS  DE BAILE////////////////////////////////
228//////////////////////////////////////////////////////////////////////////////
229
230void  goingUp(int tempo){
231  
232      pause=millis();
233      for(int i=0;i<4;i++)  servo[i].SetPosition(90);
234      delay(tempo);
235      servo[0].SetPosition(80);
236      servo[1].SetPosition(100);
237      delay(tempo);
238      servo[0].SetPosition(70);
239      servo[1].SetPosition(110);
240      delay(tempo);
241      servo[0].SetPosition(60);
242      servo[1].SetPosition(120);
243      delay(tempo);
244      servo[0].SetPosition(50);
245      servo[1].SetPosition(130);
246      delay(tempo);
247      servo[0].SetPosition(40);
248      servo[1].SetPosition(140);
249      delay(tempo);
250      servo[0].SetPosition(30);
251      servo[1].SetPosition(150);
252      delay(tempo);
253      servo[0].SetPosition(20);
254      servo[1].SetPosition(160);
255      delay(tempo);
256      
257      while(millis()<pause+8*t);
258
259}
260
261void  primera_parte(){
262  
263  int move1[4] = {60,120,90,90};
264  int move2[4] = {90,90,90,90};
265  int move3[4] = {40,140,90,90};
266  
267  for(int x=0; x<3; x++){
268    for(int  i=0; i<3; i++){
269      lateral_fuerte(1,t/2);
270      lateral_fuerte(0,t/4);
271      lateral_fuerte(1,t/4);
272      delay(t);
273    }
274  
275    pause=millis();
276    for(int i=0;i<4;i++) servo[i].SetPosition(90);
277    moveNServos(t*0.4,move1);
278    moveNServos(t*0.4,move2);
279    while(millis()<(pause+t*2));
280  }
281  
282  for(int i=0; i<2; i++){
283    lateral_fuerte(1,t/2);
284    lateral_fuerte(0,t/4);
285    lateral_fuerte(1,t/4);
286    delay(t);
287  }
288    
289  pause=millis();
290  for(int i=0;i<4;i++) servo[i].SetPosition(90);
291  crusaito(1,t*1.4);
292  moveNServos(t*1,move3);
293  for(int i=0;i<4;i++) servo[i].SetPosition(90);
294  while(millis()<(pause+t*4));
295}
296
297void  segunda_parte(){
298  
299  int move1[4] = {90,90,80,100};
300  int move2[4] = {90,90,100,80};
301  int move3[4] = {90,90,80,100};
302  int move4[4] = {90,90,100,80};
303    
304  int move5[4] = {40,140,80,100};
305  int move6[4] = {40,140,100,80};
306  int  move7[4] = {90,90,80,100};
307  int move8[4] = {90,90,100,80};
308       
309  int  move9[4] = {40,140,80,100};
310  int move10[4] = {40,140,100,80};
311  int move11[4]  = {90,90,80,100};
312  int move12[4] = {90,90,100,80};
313  
314  for(int x=0; x<7;  x++){ 
315    for(int i=0; i<3; i++){
316      pause=millis();
317      moveNServos(t*0.15,move1);
318      moveNServos(t*0.15,move2);
319      moveNServos(t*0.15,move3);
320      moveNServos(t*0.15,move4);
321      while(millis()<(pause+t));
322    }
323    pause=millis();
324    moveNServos(t*0.15,move5);
325    moveNServos(t*0.15,move6);
326    moveNServos(t*0.15,move7);
327    moveNServos(t*0.15,move8);
328    while(millis()<(pause+t));
329  }
330 
331  for(int i=0; i<3; i++){
332    pause=millis();
333    moveNServos(t*0.15,move9);
334    moveNServos(t*0.15,move10);
335    moveNServos(t*0.15,move11);
336    moveNServos(t*0.15,move12);
337    while(millis()<(pause+t));
338  }
339}
340
341void  lateral_fuerte(boolean side, int tempo){
342  
343  for(int i=0;i<4;i++) servo[i].SetPosition(90);
344  if (side) servo[0].SetPosition(40);
345  else servo[1].SetPosition(140);
346  delay(tempo/2);
347  servo[0].SetPosition(90);
348  servo[1].SetPosition(90);
349  delay(tempo/2);
350
351}
352
353void  drunk (int tempo){
354  
355  pause=millis();
356  
357  int move1[] = {60,70,90,90};
358  int move2[] = {110,120,90,90};
359  int move3[] = {60,70,90,90};
360  int move4[]  = {110,120,90,90};
361  
362  moveNServos(tempo*0.235,move1);
363  moveNServos(tempo*0.235,move2);
364  moveNServos(tempo*0.235,move3);
365  moveNServos(tempo*0.235,move4);
366  while(millis()<(pause+tempo));
367
368}
369
370
371void  noGravity(int tempo){
372  
373  int move1[4] = {120,140,90,90};
374  int move2[4]  = {140,140,90,90};
375  int move3[4] = {120,140,90,90};
376  int move4[4] = {90,90,90,90};
377  
378  
379  for(int i=0;i<4;i++) servo[i].SetPosition(90);
380  for(int i=0;i<N_SERVOS;i++)  oldPosition[i]=90;
381  moveNServos(tempo*2,move1);
382  moveNServos(tempo*2,move2);
383  delay(tempo*2);
384  moveNServos(tempo*2,move3);
385  moveNServos(tempo*2,move4);
386
387  
388
389}
390
391void kickLeft(int tempo){
392  for(int i=0;i<4;i++) servo[i].SetPosition(90);
393  delay(tempo);
394  servo[0].SetPosition(50); //pie derecho
395  servo[1].SetPosition(70);  //pie izquiero
396  delay(tempo);
397  servo[0].SetPosition(80); //pie derecho
398  servo[1].SetPosition(70); //pie izquiero
399  delay(tempo/4);
400  servo[0].SetPosition(30);  //pie derecho
401  servo[1].SetPosition(70); //pie izquiero
402  delay(tempo/4);
403  servo[0].SetPosition(80); //pie derecho
404  servo[1].SetPosition(70); //pie izquiero
405  delay(tempo/4);
406  servo[0].SetPosition(30); //pie derecho
407  servo[1].SetPosition(70);  //pie izquiero
408  delay(tempo/4);
409  servo[0].SetPosition(80); //pie derecho
410  servo[1].SetPosition(70); //pie izquiero
411  delay(tempo);
412}
413
414void kickRight(int  tempo){
415for(int i=0;i<4;i++) servo[i].SetPosition(90);
416  delay(tempo);
417  servo[0].SetPosition(110); //pie derecho
418  servo[1].SetPosition(130); //pie  izquiero
419  delay(tempo);
420  servo[0].SetPosition(110); //pie derecho
421  servo[1].SetPosition(100);  //pie izquiero
422  delay(tempo/4);
423  servo[0].SetPosition(110); //pie derecho
424  servo[1].SetPosition(150); //pie izquiero
425  delay(tempo/4);
426  servo[0].SetPosition(110);  //pie derecho
427  servo[1].SetPosition(80); //pie izquiero
428  delay(tempo/4);
429  servo[0].SetPosition(110); //pie derecho
430  servo[1].SetPosition(150); //pie  izquiero
431  delay(tempo/4);
432  servo[0].SetPosition(110); //pie derecho
433  servo[1].SetPosition(100); //pie izquiero
434  delay(tempo);
435}
436
437void  walk(int steps, int T){
438    int A[4]= {15, 15, 30, 30};
439    int O[4] = {0,  0, 0, 0};
440    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};
441    
442    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
443}
444
445void  run(int steps, int T){
446    int A[4]= {10, 10, 10, 10};
447    int O[4] = {0,  0, 0, 0};
448    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};  
449    
450    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
451}
452
453void  backyard(int steps, int T){
454    int A[4]= {15, 15, 30, 30};
455    int O[4] =  {0, 0, 0, 0};
456    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(-90),  DEG2RAD(-90)}; 
457    
458    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
459}
460
461void  backyardSlow(int steps, int T){
462    int A[4]= {15, 15, 30, 30};
463    int O[4]  = {0, 0, 0, 0};
464    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(-90),  DEG2RAD(-90)}; 
465    
466    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
467}
468
469
470void  turnLeft(int steps, int T){
471    int A[4]= {20, 20, 10, 30};
472    int O[4] =  {0, 0, 0, 0};
473    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90),  DEG2RAD(90)}; 
474    
475    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
476}
477
478void  turnRight(int steps, int T){
479    int A[4]= {20, 20, 30, 10};
480    int O[4]  = {0, 0, 0, 0};
481    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90),  DEG2RAD(90)}; 
482    
483    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
484}
485
486void  moonWalkRight(int steps, int T){
487    int A[4]= {25, 25, 0, 0};
488    int O[4]  = {-15 ,15, 0, 0};
489    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180 + 120),  DEG2RAD(90), DEG2RAD(90)}; 
490    
491    for(int i=0;i<steps;i++)oscillate(A,O,  T, phase_diff);
492}
493
494void moonWalkLeft(int steps, int T){
495    int A[4]=  {25, 25, 0, 0};
496    int O[4] = {-15, 15, 0, 0};
497    double phase_diff[4] =  {DEG2RAD(0), DEG2RAD(180 - 120), DEG2RAD(90), DEG2RAD(90)}; 
498    
499    for(int  i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
500}
501
502void crusaito(int steps,  int T){
503    int A[4]= {25, 25, 30, 30};
504    int O[4] = {- 15, 15, 0, 0};
505    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180 + 120), DEG2RAD(90), DEG2RAD(90)};  
506    
507    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
508}
509
510void  swing(int steps, int T){
511    int A[4]= {25, 25, 0, 0};
512    int O[4] = {-15,  15, 0, 0};
513    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};
514    
515    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
516}
517
518void  upDown(int steps, int T){
519    int A[4]= {25, 25, 0, 0};
520    int O[4] = {-15,  15, 0, 0};
521    double phase_diff[4] = {DEG2RAD(180), DEG2RAD(0), DEG2RAD(270),  DEG2RAD(270)};
522    
523    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
524}
525
526void  flapping(int steps, int T){
527    int A[4]= {15, 15, 8, 8};
528    int O[4] = {-A[0],  A[1], 0, 0};
529    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180), DEG2RAD(90),  DEG2RAD(-90)};
530    
531    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
532}
533
534void  test(int steps, int T){
535    int A[4]= {15, 15, 8, 8};
536    int O[4] = {-A[0]  + 10, A[1] - 10, 0, 0};
537    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180),  DEG2RAD(90), DEG2RAD(-90)};
538    
539    for(int i=0;i<steps;i++)oscillate(A,O,  T, phase_diff);
540}
541
542
543

//-----------------------------------------------------------------
#include  <Servo.h>
#include <Oscillator.h>
#include <EEPROM.h>

#define N_SERVOS  4
//-- First step: Configure the pins where the servos are attached
/*
         --------------- 
        |     O   O     |
        |---------------|
YR  3==> |               | <== YL 2
         --------------- 
            ||     ||
            ||     ||
RR 5==>   -----   ------  <== RL 4
         |-----   ------|
*/
#define  EEPROM_TRIM false 
// Activate to take callibration data from internal memory
#define  TRIM_RR 7
#define TRIM_RL 4
#define TRIM_YR  4
#define TRIM_YL -7
//OTTO.setTrims(-7,-4,-4,7);

#define  PIN_RR 5
#define PIN_RL 4
#define PIN_YR 3
#define PIN_YL 2

#define  INTERVALTIME 10.0 

Oscillator servo[N_SERVOS];

void goingUp(int tempo);
void  drunk (int tempo);
void noGravity(int tempo);
void kickLeft(int tempo);
void  kickRight(int tempo);
void run(int steps, int T=500);
void walk(int steps,  int T=1000);
void backyard(int steps, int T=3000);
void backyardSlow(int steps,  int T=5000);
void turnLeft(int steps, int T=3000);
void turnRight(int steps,  int T=3000);
void moonWalkLeft(int steps, int T=1000);
void moonWalkRight(int  steps, int T=1000);
void crusaito(int steps, int T=1000);
void swing(int steps,  int T=1000);
void upDown(int steps, int T=1000);
void flapping(int steps,  int T=1000);

void setup()
{
  Serial.begin(19200);
  
  servo[0].attach(PIN_RR);
  servo[1].attach(PIN_RL);
  servo[2].attach(PIN_YR);
  servo[3].attach(PIN_YL);
  
  //EEPROM.write(0,TRIM_RR);
  //EEPROM.write(1,TRIM_RL);
  //EEPROM.write(2,TRIM_YR);
  //EEPROM.write(3,TRIM_YL);
  
  int trim;
  
  if(EEPROM_TRIM){
    for(int x=0;x<4;x++){
      trim=EEPROM.read(x);
      if(trim>128)trim=trim-256;
      Serial.print("TRIM ");
      Serial.print(x);
      Serial.print("  en ");
      Serial.println(trim);
      servo[x].SetTrim(trim);
    }
  }
  else{
    servo[0].SetTrim(TRIM_RR);
    servo[1].SetTrim(TRIM_RL);
    servo[2].SetTrim(TRIM_YR);
    servo[3].SetTrim(TRIM_YL);
  }
  
  for(int i=0;i<4;i++) servo[i].SetPosition(90);
}

// TEMPO: 121 BPM
int  t=495;
double pause=0;

void loop()
{
 // if(Serial.available()){
  //  char init = Serial.read();
   // if (init=='X'){
   //   delay(4000);  //3000 - 4500
   
dance();


//for(int i=0;i<4;i++) servo[i].SetPosition(90);

      
          for(int i=0;i<4;i++) servo[i].SetPosition(90);
   // }
  // }
}

void dance(){
  primera_parte();
  segunda_parte();
  moonWalkLeft(4,t*2);
  moonWalkRight(4,t*2);
  moonWalkLeft(4,t*2);
  moonWalkRight(4,t*2);
  primera_parte(); 
  crusaito(1,t*8);
  crusaito(1,t*7);

  for (int  i=0; i<16; i++){
    flapping(1,t/4);
    delay(3*t/4);
  }
 
  moonWalkRight(4,t*2);
  moonWalkLeft(4,t*2);
  moonWalkRight(4,t*2);
  moonWalkLeft(4,t*2);

  drunk(t*4);
  drunk(t*4);
  drunk(t*4);
  drunk(t*4);
  kickLeft(t);
  kickRight(t);
  drunk(t*8);
  drunk(t*4);
  drunk(t/2);
  delay(t*4);  

  drunk(t/2);

  delay(t*4); 
  walk(2,t*2);
  backyard(2,t*2);
  goingUp(t*2);
  goingUp(t*1);
  noGravity(t*2);
  crusaito(1,t*2);
  crusaito(1,t*8);
  crusaito(1,t*2);
  crusaito(1,t*8);
  crusaito(1,t*2);
  crusaito(1,t*3);

  delay(t);
  primera_parte();
    for (int i=0;  i<32; i++){
    flapping(1,t/2);
    delay(t/2);
  }
  
  for(int  i=0;i<4;i++) servo[i].SetPosition(90);
}




////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////FUNCIONES  DE CONTROL//////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

void  oscillate(int A[N_SERVOS], int O[N_SERVOS], int T, double phase_diff[N_SERVOS]){
  for (int i=0; i<4; i++) {
    servo[i].SetO(O[i]);
    servo[i].SetA(A[i]);
    servo[i].SetT(T);
    servo[i].SetPh(phase_diff[i]);
  }
  double ref=millis();
   for (double x=ref; x<T+ref; x=millis()){
     for (int i=0; i<4; i++){
        servo[i].refresh();
     }
  }
}

unsigned long final_time;
unsigned  long interval_time;
int oneTime;
int iteration;
float increment[N_SERVOS];  
int oldPosition[]={90,90,90,90};

void moveNServos(int time, int  newPosition[]){
  for(int i=0;i<N_SERVOS;i++)  increment[i] = ((newPosition[i])-oldPosition[i])/(time/INTERVALTIME);
  
  final_time =  millis() + time; 
  
  iteration = 1; 
  while(millis()  < final_time){ //Javi del futuro cambia esto  
      interval_time = millis()+INTERVALTIME;  
      
      oneTime=0;      
      while(millis()<interval_time){    
          if(oneTime<1){ 
              for(int i=0;i<N_SERVOS;i++){
                  servo[i].SetPosition(oldPosition[i]  + (iteration * increment[i]));
              }     
              iteration++;
              oneTime++;
          }
      }     
  }   

  for(int  i=0;i<N_SERVOS;i++){  
    oldPosition[i] = newPosition[i];
  }   
}


//////////////////////////////////////////////////////////////////////////////
////////////////////////////////PASOS  DE BAILE////////////////////////////////
//////////////////////////////////////////////////////////////////////////////

void  goingUp(int tempo){
  
      pause=millis();
      for(int i=0;i<4;i++)  servo[i].SetPosition(90);
      delay(tempo);
      servo[0].SetPosition(80);
      servo[1].SetPosition(100);
      delay(tempo);
      servo[0].SetPosition(70);
      servo[1].SetPosition(110);
      delay(tempo);
      servo[0].SetPosition(60);
      servo[1].SetPosition(120);
      delay(tempo);
      servo[0].SetPosition(50);
      servo[1].SetPosition(130);
      delay(tempo);
      servo[0].SetPosition(40);
      servo[1].SetPosition(140);
      delay(tempo);
      servo[0].SetPosition(30);
      servo[1].SetPosition(150);
      delay(tempo);
      servo[0].SetPosition(20);
      servo[1].SetPosition(160);
      delay(tempo);
      
      while(millis()<pause+8*t);

}

void  primera_parte(){
  
  int move1[4] = {60,120,90,90};
  int move2[4] = {90,90,90,90};
  int move3[4] = {40,140,90,90};
  
  for(int x=0; x<3; x++){
    for(int  i=0; i<3; i++){
      lateral_fuerte(1,t/2);
      lateral_fuerte(0,t/4);
      lateral_fuerte(1,t/4);
      delay(t);
    }
  
    pause=millis();
    for(int i=0;i<4;i++) servo[i].SetPosition(90);
    moveNServos(t*0.4,move1);
    moveNServos(t*0.4,move2);
    while(millis()<(pause+t*2));
  }
  
  for(int i=0; i<2; i++){
    lateral_fuerte(1,t/2);
    lateral_fuerte(0,t/4);
    lateral_fuerte(1,t/4);
    delay(t);
  }
    
  pause=millis();
  for(int i=0;i<4;i++) servo[i].SetPosition(90);
  crusaito(1,t*1.4);
  moveNServos(t*1,move3);
  for(int i=0;i<4;i++) servo[i].SetPosition(90);
  while(millis()<(pause+t*4));
}

void  segunda_parte(){
  
  int move1[4] = {90,90,80,100};
  int move2[4] = {90,90,100,80};
  int move3[4] = {90,90,80,100};
  int move4[4] = {90,90,100,80};
    
  int move5[4] = {40,140,80,100};
  int move6[4] = {40,140,100,80};
  int  move7[4] = {90,90,80,100};
  int move8[4] = {90,90,100,80};
       
  int  move9[4] = {40,140,80,100};
  int move10[4] = {40,140,100,80};
  int move11[4]  = {90,90,80,100};
  int move12[4] = {90,90,100,80};
  
  for(int x=0; x<7;  x++){ 
    for(int i=0; i<3; i++){
      pause=millis();
      moveNServos(t*0.15,move1);
      moveNServos(t*0.15,move2);
      moveNServos(t*0.15,move3);
      moveNServos(t*0.15,move4);
      while(millis()<(pause+t));
    }
    pause=millis();
    moveNServos(t*0.15,move5);
    moveNServos(t*0.15,move6);
    moveNServos(t*0.15,move7);
    moveNServos(t*0.15,move8);
    while(millis()<(pause+t));
  }
 
  for(int i=0; i<3; i++){
    pause=millis();
    moveNServos(t*0.15,move9);
    moveNServos(t*0.15,move10);
    moveNServos(t*0.15,move11);
    moveNServos(t*0.15,move12);
    while(millis()<(pause+t));
  }
}

void  lateral_fuerte(boolean side, int tempo){
  
  for(int i=0;i<4;i++) servo[i].SetPosition(90);
  if (side) servo[0].SetPosition(40);
  else servo[1].SetPosition(140);
  delay(tempo/2);
  servo[0].SetPosition(90);
  servo[1].SetPosition(90);
  delay(tempo/2);

}

void  drunk (int tempo){
  
  pause=millis();
  
  int move1[] = {60,70,90,90};
  int move2[] = {110,120,90,90};
  int move3[] = {60,70,90,90};
  int move4[]  = {110,120,90,90};
  
  moveNServos(tempo*0.235,move1);
  moveNServos(tempo*0.235,move2);
  moveNServos(tempo*0.235,move3);
  moveNServos(tempo*0.235,move4);
  while(millis()<(pause+tempo));

}


void  noGravity(int tempo){
  
  int move1[4] = {120,140,90,90};
  int move2[4]  = {140,140,90,90};
  int move3[4] = {120,140,90,90};
  int move4[4] = {90,90,90,90};
  
  
  for(int i=0;i<4;i++) servo[i].SetPosition(90);
  for(int i=0;i<N_SERVOS;i++)  oldPosition[i]=90;
  moveNServos(tempo*2,move1);
  moveNServos(tempo*2,move2);
  delay(tempo*2);
  moveNServos(tempo*2,move3);
  moveNServos(tempo*2,move4);

  

}

void kickLeft(int tempo){
  for(int i=0;i<4;i++) servo[i].SetPosition(90);
  delay(tempo);
  servo[0].SetPosition(50); //pie derecho
  servo[1].SetPosition(70);  //pie izquiero
  delay(tempo);
  servo[0].SetPosition(80); //pie derecho
  servo[1].SetPosition(70); //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(30);  //pie derecho
  servo[1].SetPosition(70); //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(80); //pie derecho
  servo[1].SetPosition(70); //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(30); //pie derecho
  servo[1].SetPosition(70);  //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(80); //pie derecho
  servo[1].SetPosition(70); //pie izquiero
  delay(tempo);
}

void kickRight(int  tempo){
for(int i=0;i<4;i++) servo[i].SetPosition(90);
  delay(tempo);
  servo[0].SetPosition(110); //pie derecho
  servo[1].SetPosition(130); //pie  izquiero
  delay(tempo);
  servo[0].SetPosition(110); //pie derecho
  servo[1].SetPosition(100);  //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(110); //pie derecho
  servo[1].SetPosition(150); //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(110);  //pie derecho
  servo[1].SetPosition(80); //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(110); //pie derecho
  servo[1].SetPosition(150); //pie  izquiero
  delay(tempo/4);
  servo[0].SetPosition(110); //pie derecho
  servo[1].SetPosition(100); //pie izquiero
  delay(tempo);
}

void  walk(int steps, int T){
    int A[4]= {15, 15, 30, 30};
    int O[4] = {0,  0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void  run(int steps, int T){
    int A[4]= {10, 10, 10, 10};
    int O[4] = {0,  0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};  
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void  backyard(int steps, int T){
    int A[4]= {15, 15, 30, 30};
    int O[4] =  {0, 0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(-90),  DEG2RAD(-90)}; 
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void  backyardSlow(int steps, int T){
    int A[4]= {15, 15, 30, 30};
    int O[4]  = {0, 0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(-90),  DEG2RAD(-90)}; 
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}


void  turnLeft(int steps, int T){
    int A[4]= {20, 20, 10, 30};
    int O[4] =  {0, 0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90),  DEG2RAD(90)}; 
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void  turnRight(int steps, int T){
    int A[4]= {20, 20, 30, 10};
    int O[4]  = {0, 0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90),  DEG2RAD(90)}; 
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void  moonWalkRight(int steps, int T){
    int A[4]= {25, 25, 0, 0};
    int O[4]  = {-15 ,15, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180 + 120),  DEG2RAD(90), DEG2RAD(90)}; 
    
    for(int i=0;i<steps;i++)oscillate(A,O,  T, phase_diff);
}

void moonWalkLeft(int steps, int T){
    int A[4]=  {25, 25, 0, 0};
    int O[4] = {-15, 15, 0, 0};
    double phase_diff[4] =  {DEG2RAD(0), DEG2RAD(180 - 120), DEG2RAD(90), DEG2RAD(90)}; 
    
    for(int  i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void crusaito(int steps,  int T){
    int A[4]= {25, 25, 30, 30};
    int O[4] = {- 15, 15, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180 + 120), DEG2RAD(90), DEG2RAD(90)};  
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void  swing(int steps, int T){
    int A[4]= {25, 25, 0, 0};
    int O[4] = {-15,  15, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void  upDown(int steps, int T){
    int A[4]= {25, 25, 0, 0};
    int O[4] = {-15,  15, 0, 0};
    double phase_diff[4] = {DEG2RAD(180), DEG2RAD(0), DEG2RAD(270),  DEG2RAD(270)};
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void  flapping(int steps, int T){
    int A[4]= {15, 15, 8, 8};
    int O[4] = {-A[0],  A[1], 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180), DEG2RAD(90),  DEG2RAD(-90)};
    
    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void  test(int steps, int T){
    int A[4]= {15, 15, 8, 8};
    int O[4] = {-A[0]  + 10, A[1] - 10, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180),  DEG2RAD(90), DEG2RAD(-90)};
    
    for(int i=0;i<steps;i++)oscillate(A,O,  T, phase_diff);
}

Downloadable files

otto robot

Documentation

otto robot

Comments

Only logged in users can leave comments

xxx1997

0 Followers

•

0 Projects

Intro