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 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 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 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 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
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)
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
print('Done moving!')
dynamixel.set_led(ser, servo_id, registers.LED_STATE.ON)
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)
