diff --git a/dxl2/register.py b/dxl2/register.py
index 503725d..221bcfc 100644
--- a/dxl2/register.py
+++ b/dxl2/register.py
@@ -63,9 +63,9 @@ class Instruction(Enum):
     Instruction.MODEL_NUMBER_H: [1],
     Instruction.MODEL_NUMBER: [1, 0],
     Instruction.TORQUE_ENABLE: [24],
-    Instruction.GOAL_POSITION: [31, 30],
-    Instruction.MOVING_SPEED: [33, 32],
-    Instruction.PRESENT_POSITION: [37, 36],
+    Instruction.GOAL_POSITION: [30, 31],
+    Instruction.MOVING_SPEED: [32, 33],
+    Instruction.PRESENT_POSITION: [36, 37],
 }
 
 MX: Dict[Instruction, List[int]] = {
@@ -73,7 +73,7 @@ class Instruction(Enum):
     Instruction.MODEL_NUMBER_H: [1],
     Instruction.MODEL_NUMBER: [1, 0],
     Instruction.TORQUE_ENABLE: [24],
-    Instruction.GOAL_POSITION: [31, 30],
-    Instruction.MOVING_SPEED: [33, 32],
-    Instruction.PRESENT_POSITION: [37, 36],
+    Instruction.GOAL_POSITION: [30, 31],
+    Instruction.MOVING_SPEED: [32, 33],
+    Instruction.PRESENT_POSITION: [36, 37],
 }
diff --git a/dxl2/util.py b/dxl2/util.py
index ba4fe56..1a97f54 100644
--- a/dxl2/util.py
+++ b/dxl2/util.py
@@ -1,5 +1,4 @@
 import dynamixel_sdk as sdk
-
 from dynamixel_sdk import DXL_LOBYTE, DXL_LOWORD, DXL_HIBYTE, DXL_HIWORD
 
 
@@ -21,3 +20,14 @@ def create2ByteArray(bin_value):
 def create1ByteArray(bin_value):
     byte_array = [DXL_LOBYTE(DXL_LOWORD(bin_value))]
     return byte_array
+
+
+def angleAX(val):
+    agl =  int(512 + (val/0.29))
+    if(agl>1023):
+        return 1023
+    elif(agl<0):
+        return 0
+    else:
+        return agl
+