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 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 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 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"
def is_moving(self):
return dyn_raw.get_is_moving(dyn.dyn_serial, self.id)
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)
except Exception as e:
print('ERROR!')
print(e)
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)
except Exception as e:
print('ERROR!')
print(e)
