#include <Servo.h>

//servo objects
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;

// joystick pins
int joy1X = A0; // Joystick 1 X-axis
int joy1Y = A1; // Joystick 1 Y-axis
int joy2X = A2; // Joystick 2 X-axis
int joy2Y = A3; // Joystick 2 Y-axis

// pushbutton pin
int pushButtonPin = 2;

// pushbutton pins

int selectButton1Pin = 9;    
int selectButton2Pin = 10;    

//servo pos
int pos1, pos2, pos3, pos4, pos5, pos6;

void setup() {
  //attach servos to digi pins
  servo1.attach(3); 
  servo2.attach(4); 
  servo3.attach(5); 
  servo4.attach(6); 
  servo5.attach(7); 
  servo6.attach(8); 

  //servo pos
  pos1 = 90;
  pos2 = 90;
  pos3 = 90;
  pos4 = 90;
  pos5 = 90;
  pos6 = 90;

    pinMode(selectButton1Pin, INPUT_PULLUP);
  pinMode(selectButton2Pin, INPUT_PULLUP);

  // pushbut pin with pull-up resistor
  pinMode(pushButtonPin, INPUT_PULLUP);
}

void loop() {
  // joystick val read
  int joy1XVal = analogRead(joy1X);
  int joy1YVal = analogRead(joy1Y);
  int joy2XVal = analogRead(joy2X);
  int joy2YVal = analogRead(joy2Y);

  // Map joystick values to servo angles
  pos1 = map(joy1XVal, 0, 1023, 0, 180); 
  pos2 = map(joy1YVal, 0, 1023, 0, 180); 
  pos3 = map(joy2XVal, 0, 1023, 0, 180);
  pos4 = map(joy2YVal, 0, 1023, 0, 180); 

  // Update servo pos
  servo1.write(pos1);
  servo2.write(pos2);
  servo3.write(pos3);
  servo4.write(pos4);

    // Control servo5 with the select button of either joystick
  if (digitalRead(selectButton1Pin) == LOW) { 
    pos5 -= 10; 
    if (pos5 < 0) pos5 = 0; 
    servo5.write(pos5);
    delay(200); 
  }

  if (digitalRead(selectButton2Pin) == LOW) { 
    pos5 += 10; 
    if (pos5 > 180) pos5 = 180;
    servo5.write(pos5);
    delay(200); 
  }

  // servo6 pushbut
  if (digitalRead(pushButtonPin) == LOW) { 
    servo6.write(170); 
    delay(2000);       
    servo6.write(90);  
  }
}