#include <Servo.h>

#define HORZ_JS_PIN A3
#define VERT_JS_PIN A5
#define BUTTON_PIN 3
#define RED_LED_PIN 2
#define GREEN_LED_PIN 10
#define SWITCH_PIN 12
#define DIAL_PIN A0
#define RIGHTSERVO_PIN 9
#define LEFTSERVO_PIN 6

Servo leftservo, rightservo;  // create servo object to control a servo

int pos = 90;
int leftpos = 90;   
int rightpos = 90;   
int dialval = 0;    
int horz = 512;
int vert = 512;

void setup() {
  leftservo.attach(LEFTSERVO_PIN);  // attaches the servo on pin 9 to the servo object
  rightservo.attach(RIGHTSERVO_PIN);  // attaches the servo on pin 9 to the servo object
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  pinMode(SWITCH_PIN, INPUT);
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  pinMode(HORZ_JS_PIN, INPUT);
  pinMode(VERT_JS_PIN, INPUT);
  leftservo.write(90);              // tell servo to go to position in variable 'pos'
  rightservo.write(90);  

}

void loop() {
  // put your main code here, to run repeatedly:
  
  if  ( digitalRead(SWITCH_PIN) == HIGH )  { //the dial controls the servo positions
    //the dial controls the servo positions

    digitalWrite(RED_LED_PIN,LOW);
    digitalWrite(GREEN_LED_PIN,HIGH);
    dialval = analogRead(DIAL_PIN);            // reads the value of the potentiometer (value between 0 and 1023)
    pos = map(dialval, 0, 1023, 0, 180);     // scale it to use it with the servo (value between 0 and 180)
    //leftpos = pos;  // set the servo positions, and write them later
    rightpos = 180-pos; // set the servo positions, and write them later

  } else { 
    //the joystick controls the servo positions
  
    digitalWrite(RED_LED_PIN,HIGH);
    digitalWrite(GREEN_LED_PIN,LOW);

    if ( digitalRead(BUTTON_PIN)  == LOW ) { 
      //the green button centers the servos
   
      leftpos = 90;
      rightpos = 90;
    }
    
    // read the Joystick
    horz = analogRead(HORZ_JS_PIN);  //the joystick is an analog device
    vert = analogRead(VERT_JS_PIN);
    
    // process the joystick values to modify the servo positions
    if (vert > 520) {  // I used 520 instead of 512 to allow for a small error deadband
      leftpos ++ ;
      rightpos ++ ;
    } else if (vert < 500) { // I used 500 instead of 512 to allow for a small error deadband
      leftpos -- ;
      rightpos -- ;
    }
    if (horz > 520) {  // the horizontal joystick moves the servos in opposite directions
      leftpos -- ;
      rightpos ++ ;
    } else if (horz < 500) {
      leftpos ++ ;
      rightpos -- ;
    }
    
  }
  
  //check for limit violations on both servos in both directions
  if (leftpos > 180) {  //check for limit violations on both servos in both directions
    leftpos = 180;
  }
  if (leftpos < 0) {
    leftpos = 0;
  }
 if (rightpos > 180) {
    rightpos = 180;
  }
  if (rightpos < 0) {
    rightpos = 0;
  }

  leftservo.write(leftpos);      // tell servos to go to their positions
  rightservo.write(180-rightpos);  //the servo is rotated 180 deg, so notice: 180 - rightpos 
  delay(10);  
}