Skip to content

[dynamixel_driver] Support for Multi-turn Mode#68

Open
pazeshun wants to merge 7 commits intoarebgun:masterfrom
pazeshun:driver-multiturn
Open

[dynamixel_driver] Support for Multi-turn Mode#68
pazeshun wants to merge 7 commits intoarebgun:masterfrom
pazeshun:driver-multiturn

Conversation

@pazeshun
Copy link
Copy Markdown
Contributor

@pazeshun pazeshun commented Nov 11, 2016

What I did

  • Enable DynamixelIO to set/get multi-turn offset and resolution divider
  • Enable DynamixelIO to handle negative position value for multi-turn mode
  • Enable set_servo_config.py to change multi-turn offset and resolution divider
  • Enable info_dump.py to dump multi-turn offset and resolution divider when target servos are in multi-turn mode

@pazeshun
Copy link
Copy Markdown
Contributor Author

Tested with MX-28R
Also I confirmed I can't set multi-turn offset and resolution divider to AX-18A
No more WIP

@pazeshun pazeshun changed the title [WIP] [dynamixel_driver] Support for Multi-turn Mode [dynamixel_driver] Support for Multi-turn Mode Nov 13, 2016
@pazeshun pazeshun changed the title [dynamixel_driver] Support for Multi-turn Mode [WIP] [dynamixel_driver] Support for Multi-turn Mode Nov 14, 2016
@pazeshun
Copy link
Copy Markdown
Contributor Author

  • With bdf9bd8 , dynamixel_serial_proxy.py detects multi-turn mode and sets encoder_resolution and movable range properly

@pazeshun pazeshun changed the title [WIP] [dynamixel_driver] Support for Multi-turn Mode [dynamixel_driver] Support for Multi-turn Mode Nov 14, 2016
@pazeshun
Copy link
Copy Markdown
Contributor Author

@arebgun Please review this

# extract data values from the raw data
goal = response[5] + (response[6] << 8)
position = response[11] + (response[12] << 8)
if position & 0x8000:
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

likewise, you need this just above:

            if goal & 0x8000:
                goal += -0x10000

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks!

encoder_resolution = DXL_MODEL_TO_PARAMS[model_number]['encoder_resolution']
range_degrees = DXL_MODEL_TO_PARAMS[model_number]['range_degrees']
range_radians = math.radians(range_degrees)
if angles['max'] == 4095 and angles['min'] == 4095: # In multi-turn mode if the servo is MX motor
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

why not check DXL_MODEL_TO_PARAMS on model_number to see if multi-turn is supported rather than relying on this try .. except?

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think checking DXL_MODEL_TO_PARAMS should only be in lowest layer (ex. get_resolution_divider) for simplicity.

@dbolkensteyn
Copy link
Copy Markdown
Contributor

I've successfully tested this on the MX-12W, thank you @pazeshun !

@pazeshun
Copy link
Copy Markdown
Contributor Author

pazeshun commented Feb 1, 2017

@dbolkensteyn Thanks for review! I fixed goal of feedback.

@pazeshun pazeshun mentioned this pull request Jul 6, 2022
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants