class Emu():
"""Klasse til at kontrollere Dynamixel AX12 actuators"""
def start(self):
"""Metode til at finde USB-porten, fx COM12 på Win, ttyUSB0 på linux"""
#self.sc = Connection(port="/dev/ttyUSB0", baudrate=57600)
self.sc = Connection(port="/dev/ttyUSB0", baudrate=1000000)
#self.sc = Connection(port="COM12", baudrate=1000000)
self.sc.flush()
def scanUnits(self):
"""Scanner dynamixel motorer og returnere deres id'er i en liste"""
ids = self.sc.scan()
return ids
def readDxl(self, ID):
"""Printer oplysninger motoren med ID"""
self.sc.flush()
self.sc.pretty_print_control_table(ID)
def jointMode(self, ID):
"""Skifter motoren med ID til joint mode"""
self.sc.set_cw_angle_limit(ID, 0, False)
self.sc.set_ccw_angle_limit(ID, 1023, False)
def wheelMode(self, ID):
"""Skifter motoren med ID til wheelmode"""
self.sc.set_ccw_angle_limit(ID, 0, False)
self.sc.set_cw_angle_limit(ID, 0, False)
def moveJoint(self, ID, position):
"""Flytter motoren med ID til position"""
self.sc.goto(ID, position, speed=512, degrees=True)
time.sleep(1)
def moveWheel(self, ID, speed):
"""Starter en motor i wheelmode med hastigheden speed"""
if speed < 0:
if speed < -1024:
speed = 2047
else:
speed = 1023 + -speed
else:
if speed > 1023:
speed = 1023
self.sc.flush()
self.sc.goto(ID, 0, int(speed), degrees=False)
def stop(self):
"""Lukker forbindelsen gennem USB-porten til dynamixlerne"""
self.sc.close()
def getPos(self, ID):
"""Returnere motoren med ID's position"""
position = self.sc.get_present_position(ID, True)
return position
class DynamixelAX12(object):
def __init__(self):
# Connect to the serial port
self.connection = Connection(port="/dev/ttyUSB0", baudrate=57600)
self.dynamixel_x_axis_id = 3
self.dynamixel_z_axis_id = 4
# Setup the angle limits fot the X axis
self.connection.set_cw_angle_limit(self.dynamixel_x_axis_id, -90,
degrees=True)
self.connection.set_ccw_angle_limit(self.dynamixel_x_axis_id, 90,
degrees=True)
# Setup the angle limits fot the Z axis
self.connection.set_cw_angle_limit(self.dynamixel_z_axis_id, -90,
degrees=True)
self.connection.set_ccw_angle_limit(self.dynamixel_z_axis_id, 90,
degrees=True)
# Goto the initial position (-45°, 0°)
self.connection.goto(self.dynamixel_x_axis_id, -45, speed=255,
degrees=True)
self.connection.goto(self.dynamixel_z_axis_id, 0, speed=255,
degrees=True)
# TODO: set CW_ANGLE_LIMIT and CCW_ANGLE_LIMIT (+ alarms)
# (+ memorize the original values to set them back)
def __del__(self):
# TODO: set CW_ANGLE_LIMIT and CCW_ANGLE_LIMIT (+ alarms)
# Restore the default position (0°, 0°)
self.connection.goto(self.dynamixel_x_axis_id, 0, speed=255,
degrees=True)
self.connection.goto(self.dynamixel_z_axis_id, 0, speed=255,
degrees=True)
# Setup the angle limits for the X axis
self.connection.set_cw_angle_limit(self.dynamixel_x_axis_id,
-150, degrees=True)
self.connection.set_ccw_angle_limit(self.dynamixel_x_axis_id,
150, degrees=True)
# Setup the angle limits for the Z axis
self.connection.set_cw_angle_limit(self.dynamixel_z_axis_id,
-150, degrees=True)
self.connection.set_ccw_angle_limit(self.dynamixel_z_axis_id,
150, degrees=True)
self.connection.close()
def apply_control(self, control_vect):
pos_x = self.connection.get_present_position(self.dynamixel_x_axis_id)
pos_z = self.connection.get_present_position(self.dynamixel_z_axis_id)
print(pos_x, pos_z, control_vect)
# The x_axis controls up/down movements
new_pos_x = pos_x + int(control_vect[1] * 20.)
# The z_axis controls left/right movements
# Warning: movements are inverted on the z axis
# (negative commands move the frame to the right)
new_pos_z = pos_z - int(control_vect[0] * 20.)
speed_x = abs(int(control_vect[1] * 1023.)) # 300
speed_z = abs(int(control_vect[0] * 1023.)) # 300
try:
self.connection.goto(self.dynamixel_x_axis_id, new_pos_x, speed=speed_x)
self.connection.goto(self.dynamixel_z_axis_id, new_pos_z, speed=speed_z)
except AngleLimitError:
print("Angle limit")
class DynamixelAX12(object):
def __init__(self):
# Connect to the serial port
self.connection = Connection(port="/dev/ttyACM0", baudrate=57600)
self.dynamixel_x_axis_id = 3
self.dynamixel_z_axis_id = 4
# Setup the angle limits fot the X axis
self.connection.set_cw_angle_limit(self.dynamixel_x_axis_id,
-90,
degrees=True)
self.connection.set_ccw_angle_limit(self.dynamixel_x_axis_id,
90,
degrees=True)
# Setup the angle limits fot the Z axis
self.connection.set_cw_angle_limit(self.dynamixel_z_axis_id,
-90,
degrees=True)
self.connection.set_ccw_angle_limit(self.dynamixel_z_axis_id,
90,
degrees=True)
# Goto the initial position (-45°, 0°)
self.connection.goto(self.dynamixel_x_axis_id,
-45,
speed=255,
degrees=True)
self.connection.goto(self.dynamixel_z_axis_id,
0,
speed=255,
degrees=True)
# TODO: set CW_ANGLE_LIMIT and CCW_ANGLE_LIMIT (+ alarms)
# (+ memorize the original values to set them back)
def __del__(self):
# TODO: set CW_ANGLE_LIMIT and CCW_ANGLE_LIMIT (+ alarms)
# Restore the default position (0°, 0°)
self.connection.goto(self.dynamixel_x_axis_id,
0,
speed=255,
degrees=True)
self.connection.goto(self.dynamixel_z_axis_id,
0,
speed=255,
degrees=True)
# Setup the angle limits for the X axis
self.connection.set_cw_angle_limit(self.dynamixel_x_axis_id,
-150,
degrees=True)
self.connection.set_ccw_angle_limit(self.dynamixel_x_axis_id,
150,
degrees=True)
# Setup the angle limits for the Z axis
self.connection.set_cw_angle_limit(self.dynamixel_z_axis_id,
-150,
degrees=True)
self.connection.set_ccw_angle_limit(self.dynamixel_z_axis_id,
150,
degrees=True)
self.connection.close()
def apply_control(self, control_vect):
pos_x = self.connection.get_present_position(self.dynamixel_x_axis_id)
pos_z = self.connection.get_present_position(self.dynamixel_z_axis_id)
print(pos_x, pos_z, control_vect)
# The x_axis controls up/down movements
new_pos_x = pos_x + int(control_vect[1] * 20.)
# The z_axis controls left/right movements
# Warning: movements are inverted on the z axis
# (negative commands move the frame to the right)
new_pos_z = pos_z - int(control_vect[0] * 20.)
speed_x = abs(int(control_vect[1] * 1023.)) # 300
speed_z = abs(int(control_vect[0] * 1023.)) # 300
try:
self.connection.goto(self.dynamixel_x_axis_id,
new_pos_x,
speed=speed_x)
self.connection.goto(self.dynamixel_z_axis_id,
new_pos_z,
speed=speed_z)
except AngleLimitError:
print("Angle limit")
for i in range(0, Number_Of_Gears_Tracks + 1, 1):
Speed_Tracks[i] = (Speed_Of_First_Gear_Tracks) * i
if (
enable_position_control == 1
): #V primeru, da smo v nastavitvah nastavili vrednost na 1 se naj zgodi naslednje:
print("")
usr_in = int(
input(
"Prosim vpišite št. 1 v primeru, da želite vklopiti pozicioniranje gosenic. "
))
if (usr_in == 1):
usr_pos_1 = int(
input("Ko boste nastavili pozicijo 1, vtipkajte 1. "))
if (usr_pos_1 == 1):
pos_1_right = sc.get_present_position(7)
pos_1_left = sc.get_present_position(5)
print(str(pos_1_right))
print(str(pos_1_left))
usr_pos_2 = int(
input("Ko boste nastavili pozicijo 2, vtipkajte 2."))
if (usr_pos_2 == 2):
pos_2_right = sc.get_present_position(7)
pos_2_left = sc.get_present_position(5)
print(str(pos_2_right))
print(str(pos_2_left))
def main():
"""
A PyAX-12 demo.
Find all the currently connected AX12s and jog them back and forth through a series of turns
"""
# Parse options
parser = common_argument_parser(desc=main.__doc__)
args = parser.parse_args()
# Connect to the serial port
serial_connection = Connection(port=args.port,
baudrate=args.baudrate,
timeout=args.timeout,
rpi_gpio=args.rpi)
# Ping the dynamixel unit(s)
ids_available = serial_connection.scan()
print("Found %d servos with ids %s" % (len(ids_available), ids_available))
prev_pos = [
int(serial_connection.get_present_position(i, degrees=False))
for i in ids_available
]
print(prev_pos)
#type(prev_pos[0])
for dynamixel_id in ids_available:
print("Starting position for servo %d is %d" %
(dynamixel_id,
serial_connection.get_present_position(dynamixel_id,
degrees=True)))
# Go to -45°
serial_connection.goto(dynamixel_id, -45, speed=512, degrees=True)
print("-45 angle for servo %d is %d" %
(dynamixel_id,
serial_connection.get_present_position(dynamixel_id,
degrees=False)))
time.sleep(1) # Wait 1 second
# Go to -90° (90° CW)
serial_connection.goto(dynamixel_id, -90, speed=512, degrees=True)
time.sleep(1) # Wait 1 second
print("-90 angle for servo %d is %d" %
(dynamixel_id,
serial_connection.get_present_position(dynamixel_id,
degrees=False)))
# Go to +90° (90° CCW)
serial_connection.goto(dynamixel_id, 45, speed=512, degrees=True)
time.sleep(1) # Wait 1 second
print("+45 angle for servo %d is %d" %
(dynamixel_id,
serial_connection.get_present_position(dynamixel_id,
degrees=False)))
# Go to +90° (90° CCW)
serial_connection.goto(dynamixel_id, 90, speed=512, degrees=True)
time.sleep(1) # Wait 1 second
print("+90 angle for servo %d is %d" %
(dynamixel_id,
serial_connection.get_present_position(dynamixel_id,
degrees=False)))
serial_connection.goto(dynamixel_id, 340, speed=512, degrees=False)
time.sleep(1) # Wait 1 second
print("Home angle for servo %d is %d" %
(dynamixel_id,
serial_connection.get_present_position(dynamixel_id,
degrees=False)))
# Close the serial connection
serial_connection.close()
break
except:
pass
print("Retries: {}".format(retry))
serial_connection = Connection(port="/dev/ttyS0",
baudrate=1000000,
rpi_gpio=True)
position1 = []
init_time = time.time()
wait_time = 0.02
while time.time() - init_time < 6:
try:
# start = time.clock_gettime(time.CLOCK_MONOTONIC)
curr_pos = serial_connection.get_present_position(1)
position1.append(curr_pos)
curr_load = curr_pos + serial_connection.get_present_load(1)
new_pos = curr_pos - int(0.04 * (curr_load - curr_pos))
# - int(0.04 *serial_connection.get_present_speed(1))
print("Pos: {} Load: {} New pos {} Time {}".format(
curr_pos, curr_load, new_pos, (time.time() - init_time)))
sure_goto(1, new_pos, 400)
# while time.clock_gettime(time.CLOCK_MONOTONIC) < start+wait_time:
# pass
time.sleep(0.02)
except:
pass
sure_goto(1, position1[0], 300)
time.sleep(1)
LSU = 6 # range -113 to 40
LSS = 7 # range -49 to 81
LER = 8 # range -90 to 98
LEU = 9 # range -133 to 86
HD = 10 # range -39 to 36
speed = 200
def send_pos(mid, deg):
serial_connection.goto(mid, deg, speed, degrees=True)
time.sleep(1) #` Wait 1 second
def initall():
for dynamixel_id in range(1, 11):
serial_connection.goto(dynamixel_id, 0, speed, degrees=True)
#time.sleep(1) # Wait 1 second
# Go to 0°
## POSE 1
#dynamixel_id = RSU
#pos = -35
for mid in range(1, 11):
pos = chain.get_present_position(mid, True)
print(mid, " ", pos)
# Close the serial connection
chain.close()
