// // /*
// //  Controlling a servo position using a potentiometer (variable resistor)
// //  by Michal Rinott <http://people.interaction-ivrea.it/m.rinott>

// //  modified on 8 Nov 2013
// //  by Scott Fitzgerald
// //  http://www.arduino.cc/en/Tutorial/Knob
// // */

// // #include <Servo.h>

// // Servo myservo;  // create servo object to control a servo

// // int potpin = 0;  // analog pin used to connect the potentiometer
// // int val;    // variable to read the value from the analog pin

// // void setup() {
// //   myservo.attach(9);  // attaches the servo on pin 9 to the servo object
// // }

// // void loop() {
// //   val = analogRead(potpin);            // reads the value of the potentiometer (value between 0 and 1023)
// //   val = map(val, 0, 1023, 0, 180);     // scale it to use it with the servo (value between 0 and 180)
// //   myservo.write(val);                  // sets the servo position according to the scaled value
// //   delay(15);                           // waits for the servo to get there
// // }

#include <Servo.h>

#define JOY1XAnalogPIN  A1
#define JOY1YAnalogPIN  A0
#define JOY2XAnalogPIN  A3
#define JOY2YAnalogPIN  A2

#define SERVO1PIN      9
#define SERVO2PIN      8
#define SERVO3PIN      7
#define SERVO4PIN      6

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

int joy1X = JOY1XAnalogPIN;
int joy1Y = JOY1YAnalogPIN;
int joy2X = JOY2XAnalogPIN;
int joy2Y = JOY2YAnalogPIN;

int servo1OldValue=90;
int servo2OldValue=90;
int servo3OldValue=90;
int servo4OldValue=90;
int servoVal;
  
void setup() 
{
  pinMode(JOY1XAnalogPIN, INPUT);
  pinMode(JOY1YAnalogPIN, INPUT);
  pinMode(JOY2XAnalogPIN, INPUT);
  pinMode(JOY2YAnalogPIN, INPUT);

  servo1.attach(SERVO1PIN);
  servo2.attach(SERVO2PIN);
  servo3.attach(SERVO3PIN);
  servo4.attach(SERVO4PIN);
}
  
void loop()
{


  servoVal = analogRead(joy1X);
  if(servoVal>520 )
  {
    servo1OldValue+=1;
    if(servo1OldValue>180)
      servo1OldValue=180;
    servo1.write(servo1OldValue);
    delay(5);
  }
 if(servoVal<500 )
  {
    servo1OldValue-=1;
    if(servo1OldValue<0)
      servo1OldValue=0;
    servo1.write(servo1OldValue);
    delay(5);
  }


  servoVal = analogRead(joy1Y);
  if(servoVal>520 )
  {
    servo2OldValue+=1;
    if(servo2OldValue>180)
      servo2OldValue=180;
    servo2.write(servo2OldValue);
    delay(5);
  }
 if(servoVal<500 )
  {
    servo2OldValue-=1;
    if(servo2OldValue<0)
      servo2OldValue=0;
    servo2.write(servo2OldValue);
    delay(5);
  }


  servoVal = analogRead(joy2X);
  if(servoVal>520 )
  {
    servo3OldValue+=1;
    if(servo3OldValue>180)
      servo3OldValue=180;
    servo3.write(servo3OldValue);
    delay(5);
  }
 if(servoVal<500 )
  {
    servo3OldValue-=1;
    if(servo3OldValue<0)
      servo3OldValue=0;
    servo3.write(servo3OldValue);
    delay(5);
  }

  servoVal = analogRead(joy2Y);
  if(servoVal>520 )
  {
    servo4OldValue+=1;
    if(servo4OldValue>180)
      servo4OldValue=180;
    servo4.write(servo4OldValue);
    delay(5);
  }
 if(servoVal<500 )
  {
    servo4OldValue-=1;
    if(servo4OldValue<0)
      servo4OldValue=0;
    servo4.write(servo4OldValue);
    delay(5);
  }
  
}

// #include  <Servo.h>


// // Create servo objects 
// Servo servo1;
// Servo servo2;
// Servo servo3;
// Servo servo4;

// // Create names to the analog input pins 
// int pin0 = A0;
// int pin1 = A1;
// int pin2 = A2;
// int pin3 = A3;

// // Setup of angle variables we have to use further down in the program 
// int angle1;
// int angle2;
// int angle3;
// int angle4;

// // Setup of value variables we have to use further down in the program 
// int value1;
// int value2;
// int value3;
// int value4;

// // Setup of limit values 
// long int forservo1 = 20000;
// long int forservo2 = 20000;
// long int forservo3 = 20000;
// long int forservo4 = 20000;

// void setup() 
// { 

//   // Attaches the servo on pins to the servo objects 
//   servo1.attach(9);
//   servo2.attach(8);
//   servo3.attach(7);
//   servo4.attach(6);
// } 
 
// void loop() 
// { 
//   // read analog values from joysticks 
//   value1 = analogRead(pin0);
//   value2 = analogRead(pin1);
//   value3 = analogRead(pin2);
//   value4 = analogRead(pin3);
  
//   // 10 bit value is too big, so we change the scale from 0 - 1023 proportionately to 1 - 29 
//   value1 = map(value1, 0, 1023, 1, 29);
//   value2 = map(value2, 0, 1023, 1, 29);
//   value3 = map(value3, 0, 1023, 1, 29);
//   value4 = map(value4, 0, 1023, 1, 29);
  
//   // it needs correction of positions, because joysticks are not very precise to stay exactly in the center  
//   if(value1  <= 17 && value1 >= 13) // if value1 if between 13-17, then it's equal to 15 */
//       value1 = 15;
//   if(value2  <= 17 && value2 >= 13)
//       value2 = 15;
//   if(value3  <= 17 && value3 >= 13)
//       value3 = 15;
//   if(value4  <= 17 && value4 >= 13)
//       value4 = 15;
    
//   // Change initial value 'forservo' which is used to turn servo slower. This value has got very big scale.
//   // We add or substract (depends on turning direction) difference between analog value and center from 'forservo'  
//   forservo1 = forservo1 - (value1 - 15);
//   forservo2 = forservo2 - (value2 - 15);
//   forservo3 = forservo3 + (value3 - 15);
//   forservo4 = forservo4 + (value4 - 15);
  
//   // this part do not let variables 'forservo' go out of the limits 
//   if(forservo1  < 1) // if forservo1 is less than 1, then it's equal to 1 
//       forservo1 = 1;
//   if(forservo1  > 20000) // if forservo1 is greater than 20000, then it's equal to 20000 
//       forservo1 = 20000;
//   if(forservo2  < 1) 
//       forservo2 = 1;
//   if(forservo2  > 20000)
//       forservo2 = 20000;
//   if(forservo3  < 1) 
//       forservo3 = 1;
//   if(forservo3  > 20000)
//       forservo3 = 20000;
//   if(forservo4  < 1)
//       forservo4 = 1;
//   if(forservo4  > 20000)
//       forservo4 = 20000;
  
//   // change used value to angle of servo. Angles are adjusted during testing 
//   angle1 = map(forservo1, 1, 20000, 0, 360);
//   angle2 = map(forservo2, 1, 20000, 0, 180);
//   angle3 = map(forservo3, 1, 20000, 0, 180);
//   angle4 = map(forservo4, 1, 20000, 0, 90);
  
//   // send angles to servos 
//   servo1.write(angle1);
//   servo2.write(angle2);
//   servo3.write(angle3);
//   servo4.write(angle4);

// }