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

#include <Servo.h> 

Servo myservo;  // create servo object to control a servo 

unsigned long previousTimer1 = 0;       
unsigned long timer1 = 0;      
unsigned long previousTimerservo1 = 0;
unsigned long timerServo1 = 0;

int servoStep = 1;
int servoSpeed = 20; // total time = ( servoSpeed/servoStep ) * 180

int compass1 = 2;
int c1 = 0;
int servoPos1 = 0;
int servoMax1 = 179;
int servoMin1 = 0; 
boolean servoActive1 = false;


void setup() 
{ 
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object 
  pinMode(compass1, INPUT);
  digitalWrite(compass1, HIGH);
  Serial.begin(9600);
} 

void loop() 
{ 

  c1 = digitalRead(compass1);  // convertirlo en una funcion FOR con arrays por cada servo
  if(!c1)
    servoActive1 = true;

  //Serial.print(c1);
  //Serial.print(" ");
  //Serial.println(servoActive1);
  
  if(servoActive1)
  {

    timerServo1 = millis();
    if(timerServo1 - previousTimerservo1 > servoSpeed) 
    {
      servoPos1 = servoPos1 + servoStep;
      Serial.println(servoPos1);
      myservo.write(servoPos1);  
      if(servoPos1 >= servoMax1)
      {
        //servoPos1 = 0;
        myservo.write(servoMin1);
        //servoActive1 = false;
        servoStep = -servoStep;
      }
      else if(servoPos1 == 0)
      {
         servoStep = -servoStep;
         servoActive1 = false;
      }
      previousTimerservo1 = timerServo1; 
    }
  }



}