   if(emg1 < 100 && emg2 < 100){
     g=0;
   }
+  */
   for(int a = 13; a < 24; a++) {
     z = infoPk[13] + (infoPk[12] << 8);
     y = infoPk[15] + (infoPk[14] << 8);

  Dynamixel.setProfileVelocity(0x03, 100);
  Dynamixel.setProfileVelocity(0x04, 200);
  Dynamixel.setProfileVelocity(0x05, 200);
 }

int x, y, z, emg1, emg2, g, h, l, m, n, o;
int emgcounter = 0;
int i = 2148;
int j = 2048;

   }
   break;
}
  if(z > 460 && z < 470){
    h=0;
    }
    
while(joint == 1 && y > 700){
  i = i + 10;
  Dynamixel.setNGoalPositions(i, -1, -1, -1, -1);
  break;
  }
while(joint == 1 && y < 300){
  i = i - 10;
  Dynamixel.setNGoalPositions(i, -1, -1, -1, -1);
  break;
}

while(joint == 2 && y > 700){
  j = j + 10;
  Dynamixel.setNGoalPositions(-1, j, -1, -1, -1);
  break;
}
while(joint == 2 && y < 300){
  j = j - 10;
  Dynamixel.setNGoalPositions(-1, j, -1, -1, -1);
  break;
}

while(joint == 3 && y > 700){
  k = k + 10;
  Dynamixel.setNGoalPositions(-1, -1, k, -1, -1);
  break;
}
while(joint == 3 && y < 300){
  k = k - 10;
  Dynamixel.setNGoalPositions(-1, -1, k, -1, -1);
  break;
}

while(joint == 4 && y > 700){
    Dynamixel.gripper(OPEN);
    break;
}
while(joint == 4 && y < 300){
    Dynamixel.gripper(CLOSE);
    break;
}


/*  while(emg1 > 500 && emg1 < 1024 && g==0 && emgcounter==0){
    Dynamixel.performMovement(2148, 2048, 2048, OPEN);   //"Initial" position
    //delay(1000);
    Dynamixel.performMovement(1142, 3250, 775, OPEN);     //Ready position
