def setPosition(self, goal): m = [x for x in self.motorParams if x.name == goal.name] if len(m)>0: position=(goal.position + 2.618) * (1023 - 0) / (2.618 + 2.618) pos=np.ushort(position) dynamixel.set_position(self.bus, (m[0].busId),pos) dynamixel.send_action_packet(self.bus)
def ComprobarLista2(self):
if len(self.L_L_Goals) == 0:
return
with QtCore.QMutexLocker(self.mutex_bus):
try:
for x in range(0, len(self.L_L_Goals)):
with QtCore.QMutexLocker(self.mutex_goals):
m = self.L_L_Goals.pop(0)
for goal in m:
for x in self.motorParams:
if x.name == goal.name:
busId = x.busId
break
if x.invertedSign == "true":
goal.position = -goal.position
pos = np.ushort((goal.position + 2.618) * (1023 - 0) /
(2.618 + 2.618))
vel = np.ushort(goal.maxSpeed)
dynamixel.set_velocity(self.bus,
busId,
vel,
False,
num_error_attempts=1)
dynamixel.set_position(self.bus,
busId,
pos,
False,
num_error_attempts=1)
dynamixel.send_action_packet(self.bus)
except Ice.Exception, e:
traceback.print_exc()
print e
def setPosition(self, goal):
m = [x for x in self.motorParams if x.name == goal.name]
if len(m) > 0:
position = (goal.position + 2.618) * (1023 - 0) / (2.618 + 2.618)
pos = np.ushort(position)
dynamixel.set_position(self.bus, (m[0].busId), pos)
dynamixel.send_action_packet(self.bus)
def home():
serial_port = '/dev/ttyUSB0'
servo1_id = 1
servo2_id = 2
ser = dynamixel.get_serial_for_url(serial_port)
dynamixel.set_position(ser, servo1_id, 3071)
dynamixel.set_position(ser, servo2_id, 1010)
dynamixel.send_action_packet(ser)
tlm.zap(0)
mighty.CloseMightyZap
return ()
def ComprobarLista(self): if len(self.lisPos) == 0: return try: m = self.lisPos.popleft() for x in self.motorParams: if x.name == m.name: busId = x.busId break pos = np.ushort((m.position + 2.618) * (1023 - 0) / (2.618 + 2.618)) vel = np.ushort(m.maxSpeed) dynamixel.set_velocity(self.bus,busId, vel) dynamixel.set_position(self.bus, busId, pos) dynamixel.send_action_packet(self.bus) except Ice.Exception, e: traceback.print_exc() print e
def ComprobarLista(self):
if len(self.lisPos) == 0:
return
try:
m = self.lisPos.popleft()
for x in self.motorParams:
if x.name == m.name:
busId = x.busId
break
pos = np.ushort(
(m.position + 2.618) * (1023 - 0) / (2.618 + 2.618))
vel = np.ushort(m.maxSpeed)
dynamixel.set_velocity(self.bus, busId, vel)
dynamixel.set_position(self.bus, busId, pos)
dynamixel.send_action_packet(self.bus)
except Ice.Exception, e:
traceback.print_exc()
print e
def goToPosition(position):
servo_id1 = 1
servo_id2 = 2
try:
ser = dynamixel.get_serial_for_url(serial_port)
except:
print "failed to get motor driver"
print
velocity = 1 # slow, 4.266 seconds
dynamixel.set_position(ser, servo_id2,
int(position['TU']['LR'] * 1.0 * 1023 / 360 + 512))
dynamixel.set_position(ser, servo_id1,
int(position['TU']['UD'] * 1.0 * 1023 / 360 + 512))
dynamixel.send_action_packet(ser)
while dynamixel.get_is_moving(ser, servo_id1) == True:
velocity += velocity
dynamixel.set_velocity(ser, servo_id2, velocity)
dynamixel.set_velocity(ser, servo_id1, velocity)
dynamixel.send_action_packet(ser)
def move(sv1, sv2):
serial_port = '/dev/ttyUSB0'
servo1_id = 1
servo2_id = 2
k = 0
i = 0
ser = dynamixel.get_serial_for_url(serial_port)
dynamixel.set_velocity(ser, servo1_id, 70)
dynamixel.set_velocity(ser, servo2_id, 125)
while (i < len(sv1)):
if mt.isnan(sv1[k]) == False and mt.isnan(sv2[k]) == False:
servoPos1 = int(sv1[k])
servoPos2 = int(sv2[k])
dynamixel.set_position(ser, servo1_id, servoPos1)
dynamixel.set_position(ser, servo2_id, servoPos2)
dynamixel.send_action_packet(ser)
if k == 0:
t.sleep(0.5)
tlm.zap1(0)
t.sleep(2)
tlm.zap(0)
t.sleep(0.5)
ismoving = 1
while (ismoving == 1):
move_1 = dynamixel.get_is_moving(ser, servo1_id)
move_2 = dynamixel.get_is_moving(ser, servo2_id)
if move_1 == False or move_2 == False:
ismoving = 0
t.sleep(1)
tlm.zap(
0.6) # the variable argument is actuation length in inches
t.sleep(2)
tlm.zap(0)
t.sleep(0.75)
print('Success')
i = i + 1
k = k + 1
return ()
def ComprobarLista2(self): if len(self.L_L_Goals) == 0: return with QtCore.QMutexLocker(self.mutex_bus): try: for x in range(0, len(self.L_L_Goals)): m = self.L_L_Goals.pop(0) for goal in m: for x in self.motorParams: if x.name == goal.name: busId = x.busId break if x.invertedSign == "true": goal.position = -goal.position pos = np.ushort((goal.position + 2.618) * (1023 - 0) / (2.618 + 2.618)) vel = np.ushort(goal.maxSpeed) dynamixel.set_velocity(self.bus, busId, vel, False, num_error_attempts=1) dynamixel.set_position(self.bus, busId, pos, False, num_error_attempts=1) dynamixel.send_action_packet(self.bus) except Ice.Exception, e: traceback.print_exc() print e
def compute(self): x=len(self.lisPos) y=len(self.lisVel) if(x!=0 or y!=0): self.timer.timeout=0 try: if(x!=0): m=self.lisPos.popleft() busId=self.motores[m.name].busId pos=mapear(m.position, -1,1, 0,1023) dynamixel.set_position(self.ser, busId, pos) dynamixel.send_action_packet(self.ser) if(y!=0): m=self.lisVel.popleft() busId=self.motores[m.name].busId vel=mapear(m.velocity, 0,1, 0,1023) dynamixel.set_velocity(self.ser, busId, vel) dynamixel.send_action_packet(self.ser) except Ice.Exception, e: traceback.print_exc() print e
def multi_init_pos(t_final_sleep=2, t_sleep=0.05):
"""
Initialize snake position
:param t_init_sleep: pause time before start change goal position values.
:param t_sleep: pause time between goal position settings in each servo.
"""
# init goal position vector
global goal_pos_vect
goal_pos_vect = [
int(round(offset + amplitude_norm * sin(omega * n)))
for n in range(n_servo)
]
# Angle safe check (can slow execution)
assert abs(max(goal_pos_vect) - offset
) < 1024. / 300. * ANGLE_MAX, "Angle max (={}°) dépassé".format(
ANGLE_MAX)
for n in range(n_servo):
sleep(t_sleep)
dynamixel.set_position(ser, servo_id[n], goal_pos_vect[n])
sleep(t_final_sleep)
from pydynamixel import dynamixel
# You'll need to change this to the serial port of your USB2Dynamixel
serial_port = '/dev/ttyUSB0'
# You'll need to modify these for your setup
servo_id = 1
target_position = 768 # (range: 0 to 1023)
# If this is the first time the robot was powered on,
# you'll need to read and set the current position.
# (See the documentation above.)
first_move = True
try:
ser = dynamixel.get_serial_for_url(serial_port)
if first_move == True:
dynamixel.init(ser, servo_id)
dynamixel.set_position(ser, servo_id, target_position)
dynamixel.send_action_packet(ser)
print('Success!')
except Exception as e:
print('Unable to move to desired position.')
print(e)
# If this is the first time the robot was powered on, we need to read
# and set the current position.
# (See the documentation above.)
first_move = True
try:
ser = dynamixel.get_serial_for_url(serial_port)
# Turn the LED off
dynamixel.set_led(ser, servo_id, registers.LED_STATE.OFF)
if first_move == True:
dynamixel.init(ser, servo_id)
# Set the desired position
dynamixel.set_position(ser, servo_id, target_position)
# Set the velocity
dynamixel.set_velocity(ser, servo_id, velocity)
# Move to the desired position
dynamixel.send_action_packet(ser)
# Wait for the arm to stop moving
print('Waiting...')
# Loop until the robot is done moving.
while True:
if dynamixel.get_is_moving(ser, servo_id) == False:
break
def init_snake(
serial_port='/dev/ttyUSB0',
servo_id=[1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12],
n_period=1,
id_bloque=0, # pas de servo bloqué par défaut
angle_bloque=512,
amplitude=300,
offset=512,
t_final_sleep=2,
t_sleep=0.05):
"""
Initialize snake position for individual testing
:param serial_port: The ``serial`` object to use.
:param servo_id: the list of servo IDs.
:param n_period: number of "period" in the snake mouvement shape.
:param id_bloque: servo id of the servo which simulate a breakage.
:param angle_bloque: angle of the servo which simulate a breakage.
:param amplitude: amplitude of snake mouvement shape.
:param offset: curvature of snake mouvement shape.
:param t_final_sleep: pause time after the end of the init.
:param t_sleep: pause time between settings in each servo.
"""
ser = dynamixel.get_serial_for_url(serial_port)
n_servo = len(servo_id)
# pré-calculs
amplitude_norm = amplitude * (float(n_period) / n_servo) * 1024. / 300.
omega = 2 * pi * n_period / float(n_servo)
# enable write response for all instructions ()# gestion servo bloqué
multi_set_status_return(ser, servo_id,
registers.STATUS_RETURN.RETURN_FOR_ALL_PACKETS)
# Set velocity for init
multi_set_velocity(ser, servo_id, 200)
goal_pos_vect = [
int(round(offset + amplitude_norm * sin(omega * n)))
for n in range(n_servo)
]
# gestion servo bloqué
if (id_bloque != 0):
goal_pos_vect[servo_id.index(id_bloque)] = angle_bloque
# Angle safe check (can slow execution)
assert abs(max(goal_pos_vect) - 512
) < 1024. / 300. * ANGLE_MAX, "Angle max (={}°) dépassé".format(
ANGLE_MAX)
"""
for n in range(n_servo):
sum = 0
for z in range(n,n_servo):
sum += (goal_pos_vect[z]-512)
assert sum < 1024./300.*200. , "Le serpent allait se mordre la queue"
"""
# Move the snake in init position
for n in range(n_servo):
sleep(t_sleep)
dynamixel.set_position(ser, servo_id[n], goal_pos_vect[n])
sleep(t_final_sleep)
# Set velocity for movement
multi_set_velocity(ser, servo_id, 400)
# disable write response for read instructions
multi_set_status_return(ser, servo_id,
registers.STATUS_RETURN.RETURN_ONLY_FOR_READ)
from pydynamixel import dynamixel
# You'll need to change this to the serial port of your USB2Dynamixel
serial_port = "/dev/ttyUSB0"
# You'll need to modify these for your setup
target_position = 512 # (range: 0 to 1023)
# If this is the first time the robot was powered on,
# you'll need to read and set the current position.
# (See the documentation above.)
first_move = True
ser = dynamixel.get_serial_for_url(serial_port, baudrate=1000000)
for i in range(1, 19):
dynamixel.init(ser, i)
for i in range(1, 19):
try:
p = dynamixel.get_position(ser, i, num_error_attempts=10)
print "motor", i, "pos", p
dynamixel.set_position(ser, i, 512)
dynamixel.send_action_packet(ser)
# print('Success!')
except Exception as e:
# print('Unable to move to desired position.')
# print(e)
pass
def update_dynamixel(id, val, velocity=10):
dyn_raw.set_velocity(dyn_serial, id, velocity, verbose=True)
dyn_raw.set_position(dyn_serial, id, val, verbose=True)
dyn_raw.send_action_packet(dyn_serial)
