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)