#include <Deneyap_Servo.h>
#define SERVOPIN1 D14
#define SERVOPIN2 D9

#define L1_pin A0
#define L2_pin A1
#define L3_pin A2
#define Pot_pin A3

Servo myservo1, myservo2;

int pos1=0, pos2=0;
int accAngleX=0;
int accAngleY=0;
int conavail=0;

int coef, valin1, valin2, valin3;
int clb1=0;
int clb2=0;
int clb3=0;
int i=0;
int n=10;
int32_t tot1=0;
int32_t tot2=0;
int32_t tot3=0;

int teta1min=0;
int teta1max=180;
int teta2min=0;
int teta2max=180;

void setup() {
  myservo1.attach(SERVOPIN1);
  myservo2.attach(SERVOPIN2);

  pinMode(SW_pin,INPUT_PULLUP);
  digitalWrite(SW_pin,HIGH);
  pinMode(X_pin,INPUT);
  pinMode(Y_pin,INPUT);

}

void loop() {
  X_Val = analogRead(X_pin);
  Y_Val = analogRead(Y_pin);
  Dig_Val = digitalRead(SW_pin);

  if(Dig_Val<1) {
    if(dly==5) dly=10;
    else{
      if(dly==10) dly=20;
      else if (dly==20) dly=5;
    }

   delay(500); 
  }

  if(X_Val<1000 && pos1>teta1min) pos1-=stp;
  if(X_Val>3000 && pos1<teta1max) pos1+=stp;
  if(Y_Val<1000 && pos1>teta2min) pos2-=stp;
  if(Y_Val>3000 && pos1<teta2max) pos2+=stp;

  myservo1.write(pos1);
  myservo2.write(pos2);

  delay (dly);
}