From b50d73184f28d3d49d864d5e57a131c6459c1721 Mon Sep 17 00:00:00 2001 From: Yuki Suga Date: Sun, 23 Mar 2014 09:06:13 -0700 Subject: [PATCH 1/2] Endless Turn Mode Support --- .../dynamixel_controllers/joint_position_controller.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py b/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py index 9f5aabf..eb4b002 100644 --- a/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py +++ b/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py @@ -112,10 +112,16 @@ def pos_rad_to_raw(self, pos_rad): return self.rad_to_raw(pos_rad, self.initial_position_raw, self.flipped, self.ENCODER_TICKS_PER_RADIAN) def spd_rad_to_raw(self, spd_rad): + direction_flag = 0x0000 # Used when Endless Turn Mode + if spd_rad < 0: + if self.max_angle_raw == 0.0 and self.min_angle_raw == 0.0: # Endless Turn Mode + direction_flag = 0x0400 + spd_rad = -spd_rad + if spd_rad < self.MIN_VELOCITY: spd_rad = self.MIN_VELOCITY elif spd_rad > self.joint_max_speed: spd_rad = self.joint_max_speed # velocity of 0 means maximum, make sure that doesn't happen - return max(1, int(round(spd_rad / self.VELOCITY_PER_TICK))) + return direction_flag | max(1, int(round(spd_rad / self.VELOCITY_PER_TICK))) def set_torque_enable(self, torque_enable): mcv = (self.motor_id, torque_enable) From bf170cf9231995cb1dc889090d68b4345c019faf Mon Sep 17 00:00:00 2001 From: Yuki Suga Date: Sun, 23 Mar 2014 09:30:03 -0700 Subject: [PATCH 2/2] Speed can be zero when Endless Turn Mode --- .../joint_position_controller.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py b/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py index eb4b002..611a4b4 100644 --- a/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py +++ b/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py @@ -112,16 +112,18 @@ def pos_rad_to_raw(self, pos_rad): return self.rad_to_raw(pos_rad, self.initial_position_raw, self.flipped, self.ENCODER_TICKS_PER_RADIAN) def spd_rad_to_raw(self, spd_rad): - direction_flag = 0x0000 # Used when Endless Turn Mode - if spd_rad < 0: - if self.max_angle_raw == 0.0 and self.min_angle_raw == 0.0: # Endless Turn Mode + if self.max_angle_raw == 0.0 and self.min_angle_raw == 0.0: # Endless Turn Mode + direction_flag = 0x0000 # Used when Endless Turn Mode + if spd_rad < 0: direction_flag = 0x0400 spd_rad = -spd_rad - - if spd_rad < self.MIN_VELOCITY: spd_rad = self.MIN_VELOCITY - elif spd_rad > self.joint_max_speed: spd_rad = self.joint_max_speed - # velocity of 0 means maximum, make sure that doesn't happen - return direction_flag | max(1, int(round(spd_rad / self.VELOCITY_PER_TICK))) + if spd_rad > self.joint_max_speed: spd_rad = self.joint_max_speed + return direction_flag | int(round(spd_rad / self.VELOCITY_PER_TICK)) + else: + if spd_rad < self.MIN_VELOCITY: spd_rad = self.MIN_VELOCITY # In Endless Mode, Minimum speed is zero + elif spd_rad > self.joint_max_speed: spd_rad = self.joint_max_speed + # velocity of 0 means maximum, make sure that doesn't happen + return max(1, int(round(spd_rad / self.VELOCITY_PER_TICK))) def set_torque_enable(self, torque_enable): mcv = (self.motor_id, torque_enable)