def setVelocity(self, goal): m = [x for x in self.motorParams if x.name == goal.name] if len(m)>0: pos=np.ushort(goal.velocity) dynamixel.set_velocity(self.bus, (m[0].busId),pos) dynamixel.send_action_packet(self.bus)
def multi_set_velocity(ser, servo_id, v, t_init_sleep=0.1, t_sleep=0.05):
"""
Set velocity of the servos. Need status return enable
:param ser: The ``serial`` port to use.
:param servo_id: the list of servo IDs.
:param v: value of velocity to set.
v can be a value or a list.
:param t_init_sleep: pause time before start change reg values.
:param t_sleep: pause time between settings in each servo.
"""
n_servo = len(servo_id)
if (type(v) == int):
velocity = [v for i in range(n_servo)]
elif (type(v) == list):
if (len(v) == n_servo):
raise ValueError("lenght servo_id and lenght v are not equal")
velocity = v
else:
raise TypeError("v need to be an int or a list")
sleep(t_init_sleep)
for n in range(n_servo):
dynamixel.set_velocity(ser, servo_id[n], velocity[n])
sleep(t_sleep)
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 setVelocity(self, goal):
m = [x for x in self.motorParams if x.name == goal.name]
if len(m) > 0:
pos = np.ushort(goal.velocity)
dynamixel.set_velocity(self.bus, (m[0].busId), pos)
dynamixel.send_action_packet(self.bus)
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
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
time.sleep(0.1) # Sleep for a short amount of time (100 ms)
# Done moving
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
time.sleep(0.1) # Sleep for a short amount of time (100 ms)
# Done moving
# enable write response
for n in range(n_servo):
reg_value = registers.STATUS_RETURN.RETURN_FOR_ALL_PACKETS
reg_addr = registers.STATUS_RETURN_LEVEL # /!\ /!\ WARNING marche uniquement sur 1 bytes !!!
# goal position est sur 2 bytes !!!!
dynamixel.set_reg_1b(ser, servo_id[n], reg_addr, reg_value,
False) # revoie pas de reponse
print('Reg @{} set successfully at {} !'.format(reg_addr, reg_value))
### Velocity settings ###
v = 400
velocity = [v for i in range(n_servo)]
sleep(0.1)
for n in range(n_servo):
dynamixel.set_velocity(ser, servo_id[n], velocity[n])
#sleep(0.1)
# disable write response
for n in range(n_servo):
reg_value = registers.STATUS_RETURN.RETURN_ONLY_FOR_READ
reg_addr = registers.STATUS_RETURN_LEVEL # /!\ /!\ WARNING marche uniquement sur 1 bytes !!!
# goal position est sur 2 bytes !!!!
dynamixel.set_reg_1b(ser, servo_id[n], reg_addr,
reg_value) # revoie une reponse
print('Reg @{} set successfully at {} !'.format(reg_addr, reg_value))
# init position
goal_pos_vect = [
int(round(offset + amplitude_norm * cos(omega * n)))
for n in range(n_servo)
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)
