/*
 * Created by ArduinoGetStarted.com
 *
 * This example code is in the public domain
 *
 * Tutorial page: https://arduinogetstarted.com/tutorials/arduino-joystick
 */
#include <Servo.h>
#define VRX_PIN1  A0 // Arduino pin connected to VRX pin
#define VRY_PIN1  A1 // Arduino pin connected to VRY pin
#define VRX_PIN2  A2 // Arduino pin connected to VRX pin
#define VRY_PIN2  A3 // Arduino pin connected to VRY pin
#define VRX_PIN3  A4 // Arduino pin connected to VRX pin
#define VRY_PIN3  A5 // Arduino pin connected to VRY pin


#define LEFT_THRESHOLD  400
#define RIGHT_THRESHOLD 800
#define UP_THRESHOLD    400
#define DOWN_THRESHOLD  800

#define COMMAND1_NO     0x00
#define COMMAND1_LEFT   0x01
#define COMMAND1_RIGHT  0x02
#define COMMAND1_UP     0x04
#define COMMAND1_DOWN   0x08

#define COMMAND2_NO     0x00
#define COMMAND2_LEFT   0x01
#define COMMAND2_RIGHT  0x02
#define COMMAND2_UP     0x04
#define COMMAND2_DOWN   0x08

#define COMMAND3_NO     0x00
#define COMMAND3_LEFT   0x01
#define COMMAND3_RIGHT  0x02
#define COMMAND3_UP     0x04
#define COMMAND3_DOWN   0x08


int xValue1 = 0 ; // To store value of the X axis
int yValue1 = 0 ; // To store value of the Y axis
int xValue2 = 0 ; // To store value of the X axis
int yValue2 = 0 ; // To store value of the Y axis
int xValue3 = 0 ; // To store value of the X axis
int yValue3 = 0 ; // To store value of the Y axis

int command1 = COMMAND1_NO;
int command2 = COMMAND2_NO;
int command3 = COMMAND3_NO;


Servo myservo1;  // create servo object to control a servo
Servo myservo2;  // create servo object to control a servo
Servo myservo3;  // create servo object to control a servo
Servo myservo4;  // create servo object to control a servo
Servo myservo5;  // create servo object to control a servo
Servo myservo6;  // create servo object to control a servo

int val1 = 90; 
int val2 = 90;
int val3 = 90; 
int val4 = 90;
int val5 = 90; 
int val6 = 90;

void setup() {
  Serial.begin(9600) ;
  myservo1.attach(5);
  myservo2.attach(6);
  myservo3.attach(7);
  myservo4.attach(8);
  myservo5.attach(9);
  myservo6.attach(10);
}

void loop() {
  // read analog X and Y analog values
  xValue1 = analogRead(VRX_PIN1);
  yValue1 = analogRead(VRY_PIN1);
  xValue2 = analogRead(VRX_PIN2);
  yValue2 = analogRead(VRY_PIN2);
  xValue3 = analogRead(VRX_PIN3);
  yValue3 = analogRead(VRY_PIN3);

  // converts the analog value to commands
  // reset commands
  command1 = COMMAND1_NO;
  command2 = COMMAND2_NO;
  command3 = COMMAND3_NO;
  

  // check left/right commands
  if (xValue1 < LEFT_THRESHOLD)
    command1 = command1 | COMMAND1_LEFT;
  else if (xValue1 > RIGHT_THRESHOLD)
    command1 = command1 | COMMAND1_RIGHT;

  // check up/down commands
  if (yValue1 < UP_THRESHOLD)
    command1 = command1 | COMMAND1_UP;
  else if (yValue1 > DOWN_THRESHOLD)
    command1 = command1 | COMMAND1_DOWN;


      if (xValue2 < LEFT_THRESHOLD)
    command2 = command2 | COMMAND2_LEFT;
  else if (xValue2 > RIGHT_THRESHOLD)
    command2 = command2 | COMMAND2_RIGHT;

  // check up/down commands
  if (yValue2 < UP_THRESHOLD)
    command2 = command2 | COMMAND2_UP;
  else if (yValue2 > DOWN_THRESHOLD)
    command2 = command2 | COMMAND2_DOWN;



      if (xValue3 < LEFT_THRESHOLD)
    command3 = command3 | COMMAND3_LEFT;
  else if (xValue3 > RIGHT_THRESHOLD)
    command3 = command3 | COMMAND3_RIGHT;

  // check up/down commands
  if (yValue3 < UP_THRESHOLD)
    command3 = command3 | COMMAND3_UP;
  else if (yValue3 > DOWN_THRESHOLD)
    command3 = command3 | COMMAND3_DOWN;




/*
 *
 *
 *    SERVO 1 dan 2 / waist and shoulder
 *
 * 
 */

  // print command to serial and process command
  if (command1 & COMMAND1_LEFT) {
    Serial.println("COMMAND LEFT 1");
    val1 = val1 + 5;
    // val1++;
    myservo1.write(val1);
    Serial.println(val1);
    
    delay(50);
    // TODO: add your task here
  }

  if (command1 & COMMAND1_RIGHT) {
    Serial.println("COMMAND RIGHT 1");
    val1 = val1 - 5;
    // val1--;
    myservo1.write(val1);
    Serial.println(val1);
    
    delay(50);
    // TODO: add your task here
  }

  if (command1 & COMMAND1_UP) {
    Serial.println("COMMAND UP 1");
    val2 = val2 + 5;
    // val2++;
    myservo2.write(val2);
    Serial.println(val2);
    delay(50);
    
    // TODO: add your task here
  }

  if (command1 & COMMAND1_DOWN) {
    Serial.println("COMMAND DOWN 1");
    val2 = val2 - 5;
    // val2--;
    myservo2.write(val2);
    Serial.println(val2);
    delay(50);
    
  
  }

  /*
 *
 *
 *    SERVO 3 dan 4 / elbow and wrist rotation
 *
 * 
 */

  // print command to serial and process command
  if (command2 & COMMAND2_LEFT) {
    Serial.println("COMMAND LEFT 2");
    val3 = val3 + 5;
    // val1++;
    myservo3.write(val3);
    Serial.println(val3);
    delay(50);
    
    // delay(50);
    // TODO: add your task here
  }

  if (command2 & COMMAND2_RIGHT) {
    Serial.println("COMMAND RIGHT 2");
    val3 = val3 - 5;
    // val1--;
    myservo3.write(val3);
    Serial.println(val3);
    delay(50);
    
    // delay(50);
    // TODO: add your task here
  }

  if (command2 & COMMAND2_UP) {
    Serial.println("COMMAND UP 2");
    val4 = val4 + 5;
    // val2++;
    myservo4.write(val4);
    Serial.println(val4);
    delay(50);
    // delay(50);
    
    // TODO: add your task here
  }

  if (command2 & COMMAND2_DOWN) {
    Serial.println("COMMAND DOWN 2");
    val4 = val4 - 5;
    // val2--;
    myservo4.write(val4);
    Serial.println(val4);
    delay(50);
    
  
  }


  /*
 *
 *
 *    SERVO 5 dan 6 / wrist and gripper
 * 
 */

  // print command to serial and process command
  if (command3 & COMMAND3_LEFT) {
    Serial.println("COMMAND LEFT 3");
    val5 = val5 + 5;
    // val1++;
    myservo5.write(val5);
    Serial.println(val5);
    delay(50);
    
    // delay(50);
    // TODO: add your task here
  }

  if (command3 & COMMAND3_RIGHT) {
    Serial.println("COMMAND RIGHT 3");
    val5 = val5 - 5;
    // val1--;
    myservo5.write(val5);
    Serial.println(val5);
    delay(50);
    
    // delay(50);
    // TODO: add your task here
  }

  if (command3 & COMMAND3_UP) {
    Serial.println("COMMAND UP 3");
    val6 = val6 + 5;
    // val2++;
    myservo6.write(val6);
    Serial.println(val6);
    delay(50);
    // delay(50);
    
    // TODO: add your task here
  }

  if (command3 & COMMAND3_DOWN) {
    Serial.println("COMMAND DOWN 3");
    val6 = val6 - 5;
    // val2--;
    myservo6.write(val6);
    Serial.println(val6);
    delay(50);
    
  
  }
  
  if(val1 > 180){
    val1 = 180;
  }
  if(val1 < 0){
    val1 = 0;
  }
  if(val2 > 180){
    val2 = 180;
  }
  if(val2 < 0){
    val2 = 0;
  }
  if(val3 > 180){
    val3 = 180;
  }
  if(val3 < 0){
    val3 = 0;
  }
  if(val4 > 180){
    val4 = 180;
  }
  if(val4 < 0){
    val4 = 0;
  }
  if(val5 > 180){
    val5 = 180;
  }
  if(val5 < 0){
    val5 = 0;
  }
  if(val6 > 180){
    val6 = 180;
  }
  if(val6 < 0){
    val6 = 0;
  }
  
}