jeremiedecock / pyax12

Python library to control Dynamixel AX-12 actuators.
MIT License
19 stars 10 forks source link

Cannot put AX-12A into wheel mode #4

Closed tarandeep-panesar closed 7 years ago

tarandeep-panesar commented 7 years ago

Setting the ccw_angle_limit and cw_angle_limit both equal to 0 should put the dynamixel into a 'wheel mode' as according to the official Robotis documentation but instead an Angle Limit Error is raised.

jeremiedecock commented 7 years ago

I checked the wheel mode on AX-12+ actuators, it works.

The Angle Limit Error may be raised if you set the speed with pyax12.connection.goto() because it also sets the position (which is forbidden if the target position is not within cw/ccw angle limit range i.e. if the target is not 0).

I added a function to easily set the speed : pyax12.connection.set_speed().

Here is an example (you can also check examples/endless_turn.py ):

from pyax12.connection import Connection
import pyax12.packet as pk
import time

# Connect to the serial port
serial_connection = Connection(port="/dev/ttyAMA0", baudrate=57600, rpi_gpio=True)

dynamixel_id = 1

# Set the "wheel mode"
serial_connection.set_cw_angle_limit(dynamixel_id, 0, degrees=False)
serial_connection.set_ccw_angle_limit(dynamixel_id, 0, degrees=False)

# Activate the actuator (speed=512)
serial_connection.set_speed(dynamixel_id, 512)

# Lets the actuator turn 5 seconds
time.sleep(5)

# Stop the actuator (speed=0)
serial_connection.set_speed(dynamixel_id, 0)

# Leave the "wheel mode"
serial_connection.set_ccw_angle_limit(dynamixel_id, 1023, degrees=False)

# Go to the initial position (0 degree)
serial_connection.goto(dynamixel_id, 0, speed=512, degrees=True)

# Close the serial connection
serial_connection.close()