 void swing(int steps, int T=1000);
 void upDown(int steps, int T=1000);
 void flapping(int steps, int T=1000);
+void dance();
+void primera_parte();
+void segunda_parte();
 
 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);
+	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 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];
  }   
}



////////////////////////////////////////////////////////////////////////////////////////

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;
