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
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)
