   y = infoPk[15] + (infoPk[14] << 8);
   x = infoPk[17] + (infoPk[16] << 8);
 }
-  Serial.print(" X = ") && Serial.print(x);
+  //Serial.print(" X = ") && Serial.print(x);
   Serial.print(" Y = ") && Serial.print(y);
   Serial.print(" Z = ") && Serial.print(z);
   Serial.print(" joint number= ") && Serial.print(joint);

  Dynamixel.setHoldingTorque(0x05, true);

  //Set profile acceleration
  Dynamixel.setProfileAcceleration(0x01, 10);           //Set profile acc for each servo
  Dynamixel.setProfileAcceleration(0x02, 10);
  Dynamixel.setProfileAcceleration(0x03, 10);
  Dynamixel.setProfileAcceleration(0x04, 300);
  Dynamixel.setProfileAcceleration(0x05, 300);

  //Set profile velocity
  Dynamixel.setProfileVelocity(0x01, 100);              //Set profile vel for each servo
  Dynamixel.setProfileVelocity(0x02, 100);
  Dynamixel.setProfileVelocity(0x03, 100);
  Dynamixel.setProfileVelocity(0x04, 200);
  Dynamixel.setProfileVelocity(0x05, 200);
 }

int x, y, z, jointexcess_prevention;

   }
   break;
}
  if(z >= 420 && z <= 470){                         //Sets it back to the original position and the counter to 0
    jointexcess_prevention=0;
    }
while(joint == 1 && y > 600 && z > 300 && z < 700 && x > 0 && x < 1000){                        //Tilting forward moves joint 1 down
  i = i + 10;
  Dynamixel.setNGoalPositions(i, -1, -1, -1, -1);
  break;
}
while(joint == 1 && y < 400 && z > 300 && z < 700 && x > 0 && x < 1000){                        //Tilting backward moves joint 1 up
  i = i - 10;
  Dynamixel.setNGoalPositions(i, -1, -1, -1, -1);
  break;
}
while(joint == 2 && y > 600 && z > 300 && z < 700 && x > 0 && x < 1000){                        //Tilting forward moves joint 2 down
  j = j + 10;
  Dynamixel.setNGoalPositions(-1, j, -1, -1, -1);
  break;
}
while(joint == 2 && y < 400 && z > 300 && z < 700 && x > 0 && x < 1000){                        //Tilting backward moves joint 2 up
  j = j - 10;
  Dynamixel.setNGoalPositions(-1, j, -1, -1, -1);
  break;
}
while(joint == 3 && y > 600 && z > 300 && z < 700 && x > 0 && x < 1000){                        //Tilting forward moves joint 3 down
  k = k + 10;
  Dynamixel.setNGoalPositions(-1, -1, k, -1, -1);
  break;
}
while(joint == 3 && y < 400 && z > 300 && z < 700 && x > 0 && x < 1000){                        //Tilting backward moves joint 3 up
  k = k - 10;
  Dynamixel.setNGoalPositions(-1, -1, k, -1, -1);
  break;
}
while(joint == 4 && y > 600 && z > 300 && z < 700 && x > 0 && x < 1000){                        //Tilting forward opens the gripper
   Dynamixel.gripper(OPEN);
   break;
}
while(joint == 4 && y < 400 && z > 300 && z < 700 && x > 0 && x < 1000){                        //Tilting backward closes the gripper
   Dynamixel.gripper(CLOSE);
   break;
}
