def updateState(self): print "abasdfasfasdfasdf" for mp in self.motores: try: mpar=self.motores[mp] m=self.mstateMap[mp] m.isMoving=dynamixel.get_is_moving(self.ser,mpar.busId) pos=dynamixel.get_position(self.ser,mpar.busId) m.pos=mapear(pos, 0,1023, -1,1) except Ice.Exception, e: traceback.print_exc() print e
def readState(self): with QtCore.QMutexLocker(self.mutex_bus): for x in self.motorParams: try: state = MotorState() state.pos = float(dynamixel.get_position(self.bus, x.busId, False, num_error_attempts=1)) state.pos=(state.pos) * (2.618 + 2.618) / 1023 -2.618 if x.invertedSign == "true": state.pos=-state.pos state.isMoving = dynamixel.get_is_moving(self.bus, x.busId, False, num_error_attempts=1) self.motorStateMap[x.name] = state #state.temperature= #packet = packets.get_read_packet(m.busId,registers.PRESENT_TEMPERATURE,2) #packet = packets.get_read_packet(m.busId,registers.PRESENT_SPEED,2) #print packet except Exception, e: print e
def readState(self):
with QtCore.QMutexLocker(self.mutex_bus):
for x in self.motorParams:
try:
state = MotorState()
state.pos = float(
dynamixel.get_position(self.bus,
x.busId,
False,
num_error_attempts=1))
state.pos = (state.pos) * (2.618 + 2.618) / 1023 - 2.618
if x.invertedSign == "true":
state.pos = -state.pos
state.isMoving = dynamixel.get_is_moving(
self.bus, x.busId, False, num_error_attempts=1)
self.motorStateMap[x.name] = state
#state.temperature=
#packet = packets.get_read_packet(m.busId,registers.PRESENT_TEMPERATURE,2)
#packet = packets.get_read_packet(m.busId,registers.PRESENT_SPEED,2)
#print packet
except Exception, e:
print e
def readState(self):
for x in self.motorParams:
try:
state = MotorState()
with QtCore.QMutexLocker(self.mutex_bus):
state.pos = float(
dynamixel.get_position(self.bus,
x.busId,
False,
num_error_attempts=1))
state.pos = (state.pos) * (2.618 + 2.618) / 1023 - 2.618
state.isMoving = dynamixel.get_is_moving(
self.bus, x.busId, False, num_error_attempts=1)
if x.invertedSign == "true":
state.pos = -state.pos
state.tiempo = time.time()
self.motorStateMapWrite[x.name] = state
posV = self.motorStateMapReadOnly[x.name].pos
tiempoV = self.motorStateMapReadOnly[x.name].tiempo
if self.motorStateMapReadOnly != self.motorStateMapWrite:
state.vel = (state.pos - posV) / (state.tiempo - tiempoV)
except Exception, e:
print "Fallo leyendo motores"
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
