#include <ACB_ARM.h>//Add the Robot arm libraries
ACB_ARM ARM;

void setup() {
  ARM.ARM_init(5,16,17,18);//The parameters are four Servo pins
  ARM.JoyStick_init(32,33,34,35,36,39); // Joystick initialization
  
}
void loop() {
  ARM.get_JoyStick();
  if (ARM.JoyY1 < 50) {//chassis left
    ARM.chassis_angle = ARM.chassis_angle + 1;
    ARM.JoyChassisCmd(ARM.chassis_angle);
  }
  if (ARM.JoyY1 > 3500) {//chassis right
    ARM.chassis_angle = ARM.chassis_angle - 1;
    ARM.JoyChassisCmd(ARM.chassis_angle);
  }
  if (ARM.JoyX1 < 50) {//Shoulder down
    ARM.shoulder_angle = ARM.shoulder_angle + 1;
    ARM.JoyShoulderCmd(ARM.shoulder_angle);
  }
  if (ARM.JoyX1 > 4000) {  // Shoulder up
    ARM.shoulder_angle = ARM.shoulder_angle - 1;
    ARM.JoyShoulderCmd(ARM.shoulder_angle);
  }

  if (ARM.JoyX2 < 50) { // Elbow up 
    ARM.elbow_angle =  ARM.elbow_angle + 1;
    ARM.JoyElbowCmd(ARM.elbow_angle);
  }
  
  if (ARM.JoyX2 > 4000) { // Elbow dwon 
    ARM.elbow_angle =  ARM.elbow_angle - 1;
    ARM.JoyElbowCmd(ARM.elbow_angle);
  }

  if (ARM.JoyY2 > 4000) { // Claws open
    ARM.claws_angle = ARM.claws_angle + 1;
    ARM.JoyClawsCmd(ARM.claws_angle);
  }

  if (ARM.JoyY2 < 50) {  // Claws close
    ARM.claws_angle = ARM.claws_angle - 1;
    ARM.JoyClawsCmd(ARM.claws_angle);
  }

}

