#include <Servo.h>
#define SERVO_PIN 9
#define SERVO2_PIN 11

//define analog joystick pins, yours may have slightly different names
//depending on the joystick type, but they will essentially be the same outputs
#define GROUND_JOY_PIN A3            //joystick ground pin will connect to Arduino analog pin A3
#define VOUT_JOY_PIN A2              //joystick +5 V pin will connect to Arduino analog pin A2
#define XJOY_PIN A1                  //X axis reading from joystick will go into analog pin A1
#define YJOY_PIN A0                  //y axis reading from joystick will go into analog pin A0


//these values can be changed to set the min and max angle of the servos
//this is for the main arm, we can experiment with our indvidual robot arm
//to find the nessaracry angle constraints, to stop it from  breaking.

    int min_angle_x = 50;
    int max_angle_x = 150;
    
    //this is for the end arm of robot arm
    int min_angle_y = 1;
    int max_angle_y = 220;


//assign varible to each servo
Servo myservo ;   //x axis servo
Servo myservo1;   //y axis servo

  // create servo object to control a servo
const int analogPin = A0;    // pin that the sensor is attached to
const int ledPin = 13;       // pin that the LED is attached to
const int threshold = 400;   // set threshold level thats in the range of the analogue input                

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
 pinMode(ledPin, OUTPUT);
  // initialize serial communications:
  Serial.begin(9600);
 pinMode(VOUT_JOY_PIN, OUTPUT) ;    //pin A3 shall be used as output
  pinMode(GROUND_JOY_PIN, OUTPUT) ;  //pin A2 shall be used as output
  
  // set the outputs to either low or high ;  or 0 or 1 ;  gnd or 5v[vcc], this is 
  //all essentially the same meaning
  digitalWrite(VOUT_JOY_PIN, HIGH) ; //set pin A3 to high (+5V[VCC])
  digitalWrite(GROUND_JOY_PIN, LOW) ; //set pin A3 to low (ground)

  //attach the servo varibles to the specific pins that were defined at start
  myservo.attach(SERVO_PIN);
  myservo1.attach(SERVO2_PIN);
 pinMode(13, OUTPUT);

  
}

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

int analogValue = analogRead(analogPin);

  // if the analog value is high enough, turn on the LED:
  if (analogValue > threshold) {
    digitalWrite(ledPin, HIGH);
  } else {
    digitalWrite(ledPin, LOW);
  }

  // print the analog value:
  Serial.println(analogValue);   //print the analogValue to the serial monitor
  delay(1); 
//read the joystick values and map them to the specified angle constraints
  int joystickXVal = analogRead(XJOY_PIN) ;  //read joystick input on pin A1
  joystickXVal = map(joystickXVal, 0 ,1023 , min_angle_x , max_angle_x); // map the joystick input to min/max servo angles

  int joystickYVal = analogRead(YJOY_PIN) ;  //read joystick input on pin A0
  joystickYVal = map(joystickYVal, 0 ,1023 , min_angle_y , max_angle_y); // map the joystick input to min/max servo angles

  //move the servos to these calculated/,mapped angles 
  myservo.write(joystickXVal);  //write the mapped value to the servo
  myservo1.write(joystickYVal);  
}