 #include <DynamixelSerial.h>
 
 //Dynamixel const 
-const int DYNAMIXEL_ADDRESS = 4;
+const int DYNAMIXEL_ADDRESS = 6;
 const int DYNAMIXEL_CTRL_PIN = 2; 
+//RIGHT VALUE FOR DYNAMIXEL
 const int ANGLE_DYNAMIXEL_HORIZONTAL = 508;
-const int ANGLE_DYNAMIXEL_VERTICAL = 810;
+const int ANGLE_DYNAMIXEL_VERTICAL = 830;
 //const int ANGLE_VERTICAL = 205; Depend on the dynamixel's position 
 
 //I2C slave adresses

  //Attache the servo on the PWM pin;
  servo_clamp.attach(SERVO_CLAMP_PIN);
  servo_push.attach(SERVO_PUSH_PIN);
  servo_stack.attach(SERVO_STACK_PIN);
}

void loop() {
process_action(servo, action);
}

void receive_data(int byte_count) {


//Clamp servo const 
const int SERVO_CLAMP_PIN = 9; 
const int SERVO_PUSH_PIN = 10;
const int PUSH_BACK = 60;
const int PUSH_OUT = 25; 
const int CLAMP_OPEN_ANGLE = 70;
const int CLAMP_CLOSE_ANGLE = 180; 
Servo servo_clamp;
Servo servo_push;
 
int servo;
int action;

