From 657e4c78190379ec6990b2cd52cd7764fc9cd714 Mon Sep 17 00:00:00 2001 From: xykl0n3 Date: Sun, 3 Mar 2019 19:00:49 +0530 Subject: [PATCH 1/3] updated util functions --- dxl2/register.py | 12 ++++++------ dxl2/util.py | 7 ++++++- 2 files changed, 12 insertions(+), 7 deletions(-) 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..086ab67 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,9 @@ 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)) + return agl + From 22081a0b1a29429105e0aa3a5a60b33c1ad3c936 Mon Sep 17 00:00:00 2001 From: xykl0n3 Date: Mon, 4 Mar 2019 20:01:34 +0530 Subject: [PATCH 2/3] Update util.py --- dxl2/util.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/dxl2/util.py b/dxl2/util.py index 086ab67..4ef7494 100644 --- a/dxl2/util.py +++ b/dxl2/util.py @@ -24,5 +24,10 @@ def create1ByteArray(bin_value): def angleAX(val): agl = int(512 + (val/0.29)) - return agl + if(agl>1023): + return 1023 + elif(agl<0): + return 0) + else: + return agl From 36ffbb831855233dbb17e2f4e499a6d7e6584b4b Mon Sep 17 00:00:00 2001 From: xykl0n3 Date: Mon, 4 Mar 2019 20:01:48 +0530 Subject: [PATCH 3/3] Update util.py --- dxl2/util.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dxl2/util.py b/dxl2/util.py index 4ef7494..1a97f54 100644 --- a/dxl2/util.py +++ b/dxl2/util.py @@ -27,7 +27,7 @@ def angleAX(val): if(agl>1023): return 1023 elif(agl<0): - return 0) + return 0 else: return agl