#include <Servo.h>

Servo servo1, servo2;

int POT_PIN=A0;
int HZ=A3;
int VT=A5;

int x;
int y;


int RED_LED=2;
int BUTTON_PIN=3;
int GREEN_LED=10;
int RSERVO=6;
int LSERVO=9;
int SWITCH_PIN=12;

int potVal1;
int potVal2;

float step=1.0;
float pos=90;//sets default servo position to 90 when red side is active 
//avoids servos defaulting to extreme pos when joystick is first pressed
float pos2=90;



void setup() {

pinMode(SWITCH_PIN, INPUT);
pinMode(POT_PIN, INPUT);
pinMode(HZ, INPUT);
pinMode(VT, INPUT);
pinMode(BUTTON_PIN, INPUT);
pinMode(RSERVO, OUTPUT);
pinMode(LSERVO, OUTPUT);
pinMode(GREEN_LED, OUTPUT);
pinMode(RED_LED, OUTPUT);
servo1.attach(6);
servo2.attach(9);



}

void loop() {

if (digitalRead(SWITCH_PIN)==HIGH) 
  {
    digitalWrite(GREEN_LED,HIGH);
    digitalWrite(RED_LED, LOW);
    greenControl(); //enables green side controls 
  }
else
{
  digitalWrite(GREEN_LED, LOW);
  digitalWrite(RED_LED, HIGH);
  redControl(); //enables red side controls
}

}




void greenControl()
{
  potVal1 = analogRead(POT_PIN);  
  potVal2 = analogRead(POT_PIN);

  potVal1=map(potVal1, 0, 1023, 0, 180);
  potVal2 = map(potVal2, 0, 1023, 180, 0);
  servo1.write(potVal1);
  servo2.write(potVal2);

  delay(15);
}



void redControl()
{

if (digitalRead(BUTTON_PIN)==HIGH) //if button pressed reorient servo to 90
  {//this button is VERY buggy! it usually sticks and must be clicked several times to work
    
    servo1.write(90);  
    servo2.write(90);   
  }
else 
{
  servoControl();       // if button not pressed, control servos with joystick
}
}




void servoControl()
{

y=analogRead(VT);
x=analogRead(HZ);

if (y==512){HZ_CONTROL();}//if vertical button is not pressed, horizontal control is enabled
else{VT_CONTROL();} //otherwise vertical control function is enabled
}


void VT_CONTROL()  //vertical control fns
{

if (y>512)
{

if(pos>180){pos=180;}
if(pos<0){pos=0;}
if(pos2>180){pos2=180;}
if(pos2<0){pos2=0;}

pos ++;
pos2 --;
delay(10);
 servo1.write(pos+1);
 servo2.write(pos2-1);

}
if (y<512)
{

if(pos>180){pos=180;}
if(pos<0){pos=0;}
if(pos2>180){pos2=180;}
if(pos2<0){pos2=0;}
pos --;
pos2 ++;
delay(10);
servo1.write(pos);
servo2.write(pos2);
servoControl();

  }

}





void HZ_CONTROL() //horizontal control fns
{

  if (x>512)  
{
if(pos>180){pos=180;}
if(pos<0){pos=0;}
if(pos2>180){pos2=180;}
if(pos2<0){pos2=0;}

pos -=step;
pos2 -=step;
delay(10);
servo1.write(pos); 
servo2.write(pos2);


}
  if (x<512)
{

if(pos>180){pos=180;}  
if(pos<0){pos=0;}
if(pos2>180){pos2=180;}
if(pos2<0){pos2=0;}
pos +=step;
pos2 +=step;
delay(10);
servo1.write(pos);
servo2.write(pos2);

  servoControl();

}
}