#include <Servo.h>
Servo servo_1;
Servo servo_2;
int LED_1 = 8;
int LED_2 = 9;
int X_pin = A0;
int Y_pin = A1;
int X_Angle;
int Y_Angle;
int sw_1 = 2;
int sw_2 = 3;
bool SW_1;
bool SW_2;
bool state_1 = false;
bool state_2 = false;
int d1 = 90;
int d2 = 90;
unsigned long time1 = 0;
unsigned long time2 = 0;
unsigned long time3;
unsigned long time4;
int FAWAZ(int value , bool revers){
if(value >= 512){
value = map(value,512,1023,90,180);
}
else if(value <= 512){
value = map(value,512,0,90,0);
}
else{
value = 90;
}
if(revers){
value = (180-value);
}
return value; 
}
void setup(){
servo_1.attach(5);
servo_2.attach(6);
Serial.begin(9600);
pinMode(sw_1 , INPUT_PULLUP);
pinMode(sw_2 , INPUT_PULLUP);
pinMode(LED_1 , OUTPUT);
pinMode(LED_2 , OUTPUT);
}
void loop(){
time3 = millis();
time4 = millis();
SW_1 = !digitalRead(sw_1);
SW_2 = !digitalRead(sw_2);
if(time3 - time1 >= 200){
if(SW_1 == HIGH){
state_1 = !state_1;
digitalWrite(LED_1 , state_1);
}
if(SW_2 == HIGH){
state_2 = !state_2;
digitalWrite(LED_2 , state_2);
}
time1 = time3;
}
X_Angle = FAWAZ(analogRead(X_pin),state_1);
Y_Angle = FAWAZ(analogRead(Y_pin),state_2);
d1 = d1+(X_Angle-d1)/2;
d2 = d2+(Y_Angle-d2)/2;
servo_1.write(d1);
servo_2.write(d2);
if(time4 - time2 >= 1000){
Serial.println("__________________________________________________________");
Serial.print("| ");
Serial.print("X_Angle : ");
Serial.print(X_Angle);
Serial.print(" | Y_Angle : ");
Serial.print(Y_Angle);
Serial.print(" | State_1 : ");
Serial.print(state_1);
Serial.print(" | State_2 : ");
Serial.print(state_2);
Serial.println(" |");
Serial.println("__________________________________________________________");
time2 = time4;
}
}