#include <Servo.h>

#define VSPEED 8
#define HSPEED 10

  Servo horservo;  // create servo object to control a servo
  Servo verservo;  // a maximum of eight servo objects can be created

  int verpos = 0;    // variable to store the vertical servo position
  int horpos = 0;    // variable to store the horizontal servo position

void setup(){
  horservo.attach(9);  // attaches the horizontal servo on pin 9 to the servo object
  verservo.attach(8);  // attaches the vertical servo on pin 9 to the servo object
  horservo.write(90);
  verservo.write(0);
  Serial.begin(9600);
}

//void motion(){
//  for(; horpos <= 180; horpos += 10){     // goes from 0 degrees to 180 degrees in steps of 1 degree
//    for(; verpos <= 90; verpos++){
//      verservo.write(verpos);
//      delay(VSPEED);
//    }
//    for(; verpos >= 0; verpos--){
//      verservo.write(verpos);
//      delay(VSPEED);
//    }
//    horservo.write(horpos);             // tell servo to go to position in variable 'pos'
//    delay(HSPEED);                           // waits 15ms for the servo to reach the position
//  }
//
//  for(; horpos >= 0; horpos -= 10){     // goes from 0 degrees to 180 degrees in steps of 1 degree
//    for(; verpos <= 90; verpos++){
//      verservo.write(verpos);
//      delay(VSPEED);
//    }
//    for(; verpos >= 0; verpos--){
//      verservo.write(verpos);
//      delay(VSPEED);
//    }
//    horservo.write(horpos);              // tell servo to go to position in variable 'pos'
//    delay(HSPEED);                       // waits 15ms for the servo to reach the position
//  }
//}

void Print_Data(){
    Serial.print("Vert Pos: ");
    Serial.print(verpos);
    Serial.print(",");
    Serial.print("Hor Pos ");
    Serial.print(horpos);
    Serial.print('\n');
}

void motion_1(){
  for(; horpos < 180; horpos += 2){
    horservo.write(horpos);
    if(verpos >= 180){
      verservo.write(--verpos);
      delay(VSPEED);
      Print_Data();
    }
    else if (verpos <= 90){
      verservo.write(++verpos);
      delay(VSPEED);
      Print_Data();
    }
    delay(HSPEED);
    Print_Data();
  }
  for(; horpos > 0; horpos -= 2){
    horservo.write(horpos);
    if(verpos >= 180){
      verservo.write(--verpos);
      delay(VSPEED);
      Print_Data();
    }
    else if (verpos <= 90){
      verservo.write(++verpos);
      delay(VSPEED);
      Print_Data();
    }
    delay(HSPEED);
    Print_Data();
  }
}

void loop(){
  motion_1();
}