#include <Servo.h>

Servo myservo1;  // create servo object to control a servo
Servo myservo2;  // create servo object to control a servo

#define btn1 5
#define btn2 4
#define btn3 3
int home_pos2 = 80;
int home_pos1 = 60;
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Serial.println("Begin.");
  myservo1.attach(11);
  myservo2.attach(10);
  attachInterrupt(digitalPinToInterrupt(2),btn2Pressed,RISING); 
  pinMode(btn1, INPUT_PULLUP);
  // pinMode(btn2, INPUT_PULLUP);
  pinMode(btn3, INPUT_PULLUP);
}
int pos1 = 90;
int pos2 = 90;
int i =0;
void loop() {
  // put your main code here, to run repeatedly:
  int s1 = digitalRead(btn1);
  int s3 = digitalRead(btn3);

  if (s1 == 0){
    i = 0;
    myservo1.write(pos1);
    myservo2.write(pos2);
    // for (int i = 0; i<5;i++){
    while (i <5){
      Serial.println("S1");
      pos1 += 30;
      pos2 += 10;
      myservo2.write(pos2);
      delay(100);
      myservo1.write(pos1);
      delay(100);
      pos1 -= 30;
      pos2 -= 10;
      myservo2.write(pos2);
      delay(100);
      myservo1.write(pos1);
      delay(100);
      i+=1;
    }
  }
  if (s3 == 0){
    i = 0;
    myservo1.write(pos1);
    myservo2.write(pos2);
    // for (int i = 0; i<5;i++){
    while (i <5){
      Serial.println("S3");
      pos1 += 30;
      pos2 += 10;
      myservo1.write(pos1);
      delay(100);
      myservo2.write(pos2);
      delay(100);
      pos1 -= 30;
      pos2 -= 10;
      myservo1.write(pos1);
      delay(100);
      myservo2.write(pos2);
      delay(100);
      i+=1;
    }
  }
  
}
void btn2Pressed()          
{
  Serial.println("Interrupt");
  i = 5;
  myservo1.write(home_pos1);
  myservo2.write(home_pos2);
  delay(1000);
}