class TorsoJointPublisher:
def __init__(self):
self.dxl = DynamixelIO('/dev/ttyUSB1', baudrate = 9600)
rate = rospy.get_param('~rate', 60)
self.r = rospy.Rate(rate)
self.pub = rospy.Publisher('/bender/torso/joint_states', JointState, queue_size=10)
rospy.loginfo("Starting Torso Joint State Publisher at " + str(rate) + "Hz")
self.msg = JointState()
self.msg.name = ["torso_lift_joint"]
self.msg.velocity = [0.0]
self.msg.effort = [0.0]
def publish_joint_states(self, pos):
# Construir mensaje
try:
value = self.dxl.read(15, 6, 1)[-3]
#print value
except:
value = 100
self.msg.header.stamp = rospy.Time.now()
self.msg.position = [value*1.0/200.0*0.65-0.25]
self.pub.publish(self.msg)
#print self.msg
self.r.sleep()
def __init__(self):
self.dxl = DynamixelIO('/dev/ttyUSB1', baudrate = 9600)
rate = rospy.get_param('~rate', 60)
self.r = rospy.Rate(rate)
self.pub = rospy.Publisher('/bender/torso/joint_states', JointState, queue_size=10)
rospy.loginfo("Starting Torso Joint State Publisher at " + str(rate) + "Hz")
self.msg = JointState()
self.msg.name = ["torso_lift_joint"]
self.msg.velocity = [0.0]
self.msg.effort = [0.0]
def check(self):
SystemCheck.print_high("Target device : " + self.device, 1)
SystemCheck.print_high("Target baudrate: %d" % self.baud, 1)
try:
SystemCheck.print_info("Opening serial port... ", 1)
dxl = DynamixelIO(self.device, self.baud)
except:
SystemCheck.print_error(
"Error opening: {} [baud {}]\t\t[FAIL]".format(
self.device, self.baud))
return False
SystemCheck.print_info("Checking ids : %s" % self.ids, 1)
result = True
found_device = set()
required = set(self.ids)
for device_id in self.ids:
for trial in range(5):
try:
result = dxl.ping(device_id)
except Exception as ex:
SystemCheck.print_error(
"Exception thrown while pinging device id {}.\t\t[FAIL]"
.format(device_id))
SystemCheck.print_error(str(ex))
if result:
SystemCheck.print_ok("Motor id: {}".format(device_id), 2)
found_device.add(device_id)
break
if not required.issubset(found_device):
not_found = required.difference(found_device)
for device_id in not_found:
SystemCheck.print_error(
"Motor id: {} not found".format(device_id), 2)
result = False
return result
def main():
dxl = DynamixelIO('/dev/ttyUSB0', baudrate = 9600)
# command = int(sys.argv[1])
# print 'Command: ' + str(command)
# [0xFF, 0xFF, ID, Length, Instruction, Parameters, Checksum]
test_ping(dxl, 5)
#test_write(dxl, 15)
print dxl.ping(5)
print dxl.ping(5)
print dxl.ping(5)
while True:
dxl.write(5,13,[136])
time.sleep(5)
if new_id < 32:
print 'Not Allow ID:%d. Must be greater than 31' % (new_id)
return
self.write_addr(3, new_id)
def read_tactil_sensor(self, id):
value_lb = 0
value_hb = 0
if id == 1:
value_lb = self.get_state(TACTIL1_ADDR_LB)
value_hb = self.get_state(TACTIL1_ADDR_HB)
elif id == 2:
value_lb = self.get_state(TACTIL2_ADDR_LB)
value_hb = self.get_state(TACTIL2_ADDR_HB)
else:
print 'Not Allow ID:%d. Must be 1 or 2' % (id)
return value_lb | (value_hb << 8)
if __name__ == '__main__':
DEV_ID = 36
dxl = DynamixelIO('/dev/ttyUSB0', baudrate=1000000)
hand = HandInterface(dxl, dev_id=DEV_ID)
# print dxl.ping(DEV_ID)
# hand.change_id(36)
while True:
t1 = hand.read_tactil_sensor(1)
t2 = hand.read_tactil_sensor(2)
print 'Tactil sensor L:%d \t\t\t Tactil sensor R:%d' % (t1, t2)
time.sleep(0.1)
def main():
dxl = DynamixelIO('/dev/ttyUSB2', baudrate = 200000)
# print dxl.write(1,3,[35])
# time.sleep(0.1)
print dxl.ping(35)
