// Controlling a servo position with speed control
// by Alex Posada

#include <Servo.h> 

#define debug  1

Servo myservo[5]; 

unsigned long timer[5] = {
  0,0,0,0,0};      
unsigned long prevTimer[5] = {
  0,0,0,0,0};  

int servoStep[5] = {
  1,1,1,1,1}; 
int servoSpeed[5] ={
  20,20,20,20,20};  // total time = ( servoSpeed/servoStep ) * 180

int compass[5] = {
  2,3,4,5,6}; 
int servoPin[5] = {
  7,8,9,10,11}; 
int sensor[5] = {
  0,0,0,0,0}; 
int servoPos[5] = {
  0,0,0,0,0}; 
int servoMax[5] = {
  179,179,179,179,179}; 
int servoMin[5] = {
  0,0,0,0,0}; 
boolean servoActive[5] = {
  0,0,0,0,0}; 


void setup() 
{

  for(int i=0 ; i<5 ; i++)
  {
    myservo[i].attach(servoPin[i]);  // attaches the servo on pin 9 to the servo object 
    pinMode(compass[i], INPUT);
    digitalWrite(compass[i], HIGH);

  }

  Serial.begin(9600);

} 

void loop() 
{ 

  for(int i=0 ; i<5 ; i++)
  {
    sensor[i] = digitalRead(compass[i]);  // convertirlo en una funcion FOR con arrays por cada servo
    if(!sensor[i])
      servoActive[i] = true;

    if(servoActive[i])
    {

      timer[i] = millis();
      if(timer[i] - prevTimer[i] > servoSpeed[i]) 
      {
        servoPos[i] = servoPos[i] + servoStep[i];
  
        if(debug)
        {
          Serial.print("Servo");
          Serial.print(i+1);
          Serial.print(" = ");
          Serial.println(servoPos[i]);
        }
        
        myservo[i].write(servoPos[i]);   
        if(servoPos[i] >= servoMax[i])
        {
          myservo[i].write(servoMin[i]);
          servoStep[i] = -servoStep[i];
        }
        else if(servoPos[i] == 0)
        {
          servoStep[i] = -servoStep[i];
          servoActive[i] = false;
        }
        prevTimer[i] = timer[i]; 
      }
    }
  }
}