def __init__(self, aqc_info, sesh_path, bb, cal_mod): # create numpy array to store data initially from board self.tmp_dat = np.empty((1, N_S)) self.sens_dat = np.empty((0, N_S)) self.time_dat = np.empty((0, 2)) self.cop_dat = np.empty((0, 2)) # store session path self.sesh_path = sesh_path # create var to store start and end time save_start = 0 save_end = 0 # set save data to file and array flag self.savedatf = False self.storedat = False # input acquisition info self.acq_info = aqc_info #initializing wii and poll objects self.p_obj = select.poll() self.bbdev = xwiimote.iface(bb.sys_path) # register bbdev to pollong object self.p_obj.register(self.bbdev.get_fd(), select.POLLIN) # open bb device self.bbdev.open(xwiimote.IFACE_BALANCE_BOARD) # event structure self.revt = xwiimote.event() # calibration model self.cal_mod = cal_mod
def __init__(self, tracker = None, probeYaw = False, probePitch = False): # ------------ Connecting to wii remote. self.mote = None self.tracker = tracker self.monitor = xwiimote.monitor(True, True) while self.mote == None: self.mote = self.monitor.poll() rospy.sleep(1) # ------------ Setting up wii remote. self.mote = xwiimote.iface(self.mote) self.mote.open(self.mote.available() | xwiimote.IFACE_WRITABLE) self.mote.open(self.mote.available() | xwiimote.IFACE_ACCEL) self.mote.open(self.mote.available() | xwiimote.IFACE_MOTION_PLUS) self.poller = poll() self.poller.register(self.mote.get_fd(), POLLIN) self.event = xwiimote.event() # ------------ Parameters, data, etc. self.current = [0,0,0] self.resting = [0,0,0] self.last_reading = rospy.Time.now() self.last_button_press = rospy.Time.now() self.target = [30, 0, -20] # just for testing purposes self.index = 0 # ditto self.rumble_proportion = 0 self.probeYaw = probeYaw self.probePitch = probePitch # ----------- ROS Publishers/subscribers. self.button_pub = rospy.Publisher("/wii_buttons", Int32, queue_size=10) self.orientation_pub = rospy.Publisher("/wii_orientation", Int32MultiArray, queue_size=10) rospy.Subscriber("/wii_rumble", Float32, self.set_rumble)
def GetMeasurementsFromBoard(iface, board_p): board_p.poll(16) # with no argument, it blocks event = xwiimote.event() iface.dispatch(event) tl = event.get_abs(2)[0] tr = event.get_abs(0)[0] bl = event.get_abs(3)[0] br = event.get_abs(1)[0] return numpy.array((tl, tr, bl, br)) / 100.0
def weight_gen(): with get_balance_board_iface() as bb: p = select.poll() p.register(bb.get_fd(), select.POLLIN) event = xwiimote.event() while True: p.poll() bb.dispatch(event) decigrams = sum(x[0] for x in (event.get_abs(i) for i in range(4))) yield decigrams / 100.0
def measurements(dev): p = select.epoll.fromfd(dev.get_fd()) while True: p.poll() # blocks event = xwiimote.event() dev.dispatch(event) fl = event.get_abs(2)[0] fr = event.get_abs(0)[0] br = event.get_abs(3)[0] bl = event.get_abs(1)[0] yield fl, fr, br, bl
def measurment(self): if not init_pool: self._init_pool() self.p.poll() event = xwiimote.event() t = time.time() self.iface.dispatch(event) tl = event.get_abs(2)[0] tr = event.get_abs(0)[0] br = event.get_abs(3)[0] bl = event.get_abs(1)[0] return [tl, tr, br, bl], t
def __init__(self): self.mote = None self.monitor = xwiimote.monitor(True, True) while self.mote == None: self.mote = self.monitor.poll() rospy.sleep(1) self.mote = xwiimote.iface(self.mote) self.mote.open(self.mote.available() | xwiimote.IFACE_WRITABLE) self.poller = poll() self.poller.register(self.mote.get_fd(), POLLIN) self.event = xwiimote.event()
def run(self): time.sleep(0.1) while self.run_flag: self.p.poll() event = xwiimote.event() try: self.dev.dispatch(event) except BlockingIOError as e: print(e) continue # 0, 1, 2, 3 # FR, BR, FL, BL self.m = np.array([event.get_abs(i)[0] for i in range(N_SENS)])
def calcularPeso(mdevice, p1, calibrations): calibration = [] for i in xrange(4): calibration.append(0) p1.poll() # blocks event = xwiimote.event() mdevice.dispatch(event) calibration[0] = event.get_abs(0)[0] calibration[3] = event.get_abs(1)[0] calibration[2] = event.get_abs(2)[0] calibration[1] = event.get_abs(3)[0] for i in xrange(50): p1.poll() # blocks event = xwiimote.event() mdevice.dispatch(event) calibration[0] = (calibration[0] + event.get_abs(0)[0]) / 2 calibration[3] = (calibration[3] + event.get_abs(1)[0]) / 2 calibration[2] = (calibration[2] + event.get_abs(2)[0]) / 2 calibration[1] = (calibration[1] + event.get_abs(3)[0]) / 2 named_calibration = { 'right_top': calibration[0], 'right_bottom': calibration[1], 'left_top': calibration[2], 'left_bottom': calibration[3], } print(calibration) print(named_calibration) print("terminada calibracion") #p1.close() return named_calibration
def measurements(iface): p = select.epoll.fromfd(iface.get_fd()) while True: p.poll() # blocks event = xwiimote.event() iface.dispatch(event) tl = event.get_abs(2)[0] tr = event.get_abs(0)[0] br = event.get_abs(3)[0] bl = event.get_abs(1)[0] yield (tl, tr, br, bl)
def measurements(iface): p = select.epoll.fromfd(iface.get_fd()) while True: p.poll() # blocks event = xwiimote.event() iface.dispatch(event) tl = event.get_abs(2)[0] tr = event.get_abs(0)[0] br = event.get_abs(3)[0] bl = event.get_abs(1)[0] yield (tl,tr,br,bl)
def procBBdata(bb, func, *args): # function to get data from BB and process it function 'func' # returns sens_dat that is an NX4 numpy array of raw sensor readings # each row a single data acquisition n_s = 4 bbdev = xwiimote.iface(bb.sys_path) p = select.poll() p.register(bbdev.get_fd(), select.POLLIN) # open bb device bbdev.open(xwiimote.IFACE_BALANCE_BOARD) # create xwiimote event structure revt = xwiimote.event() # create numpy array to store data from board tmp_dat = np.empty((1, n_s)) # creat another to accumulate all data sens_dat = np.empty((0, n_s)) go_flg = True try: while go_flg: # waits here until event occurs polls = p.poll() for fd, evt in polls: try: bbdev.dispatch(revt) # revt.get_abs() takes an integer argument: # 0 - top right when power button pointing towards you # 1 - bottom right # 2 - top left # 3 - bottom left for i_s in range(n_s): tmp_dat[0, i_s] = revt.get_abs(i_s)[0] # function that does something with sens_dat. Functions # called by procBBdata must have parameters: # go_flg, tmp_dat, sens_dat go_flg, sens_dat = func(go_flg, tmp_dat, sens_dat, *args) except IOError as e: # do nothing if resource unavailable if e.errno != errno.EAGAIN: print(e) p.unregister(bbdev.get_fd()) except KeyboardInterrupt: pass # cleaning bbdev.close(xwiimote.IFACE_BALANCE_BOARD) p.unregister(bbdev.get_fd()) return sens_dat
def measurment(self): """Return 5 values: - top_left sensor value (int) - top_right sensor value (int) - bottom_right sensor value (int) - bottom_left sensor value (int) - timestamp (float)""" if not self.init_pool: self._init_pool() self.p.poll() event = xwiimote.event() t = time.time() self.iface.dispatch(event) tl = event.get_abs(2)[0] tr = event.get_abs(0)[0] br = event.get_abs(1)[0] bl = event.get_abs(3)[0] return tl, tr, br, bl, t
def __init__(self, bb, cal_mod, BB_X, BB_Y): threading.Thread.__init__(self) self.runflag = True self.storeflag = False self.n_s = 4 self.bbdev = xwiimote.iface(bb.sys_path) self.p = select.poll() self.p.register(self.bbdev.get_fd(), select.POLLIN) # open bb device self.bbdev.open(xwiimote.IFACE_BALANCE_BOARD) # create xwiimote event structure self.revt = xwiimote.event() # create numpy array to store data from board self.tmp_dat = np.empty((1, self.n_s)) self.cop = np.empty((1, 2)) self.cop_dat = np.empty((0, 2)) self.cal_mod = cal_mod self.BB_X = BB_X self.BB_Y = BB_Y
def handle_wiimote(self): evt = xwiimote.event() while True: self.poller.poll() try: self.wiimote.dispatch(evt) if evt.type == xwiimote.EVENT_KEY: code, state = evt.get_key() self.handle_key(code, state) elif evt.type == xwiimote.EVENT_GONE: print("Wiimote is gone! Don't panic!") return elif evt.type == xwiimote.EVENT_WATCH: print("Watch") except IOError as e: if e.errno != errno.EAGAIN: print("Bad")
def __init__(self, aqc_info, sesh_path, bb, cal_mod): # aqc_info: dictionary of acquisition specific info # sesh_path: path to session directory # bb: a pyudev device object for balance board # cal_mod: calibration model # create new variables and arrays # create numpy array to store data initially from board self.tmp_dat = np.empty((1, N_S)) # create numpy array to accumulate raw sensor_data self.sens_dat = np.empty((0, N_S)) # create numpy array to store sensor acq time data self.time_dat = np.empty((0, 2)) # create numpy array to store COP data self.cop_dat = np.empty((0, 2)) # store session path self.sesh_path = sesh_path # create var to store save start time save_start = 0 # create var to store save end time save_end = 0 # set save data to file flag self.savedatf = False # set store data to array flag self.storedat = False # input acquisition info self.acq_info = aqc_info # create polling object self.p_obj = select.poll() # create xwiimote bboard device self.bbdev = xwiimote.iface(bb.sys_path) # register bbdev to pollong object self.p_obj.register(self.bbdev.get_fd(), select.POLLIN) # open bb device self.bbdev.open(xwiimote.IFACE_BALANCE_BOARD) # event structure self.revt = xwiimote.event() # calibration model self.cal_mod = cal_mod
def measurements(iface, p, calibration): while True: p.poll() # blocks event = xwiimote.event() iface.dispatch(event) tl = event.get_abs(2)[0] - calibration['left_top'] tr = event.get_abs(0)[0] - calibration['right_top'] br = event.get_abs(3)[0] - calibration['right_bottom'] bl = event.get_abs(1)[0] - calibration['left_bottom'] if tl < 0: tl = 0 if tr < 0: tr = 0 if bl < 0: bl = 0 if br < 0: br = 0 if (tl + tr + br + bl) > 500: # tl = tr = br = bl = 0 #sleep(0.5) yield (tl,tr,br,bl)
def read(self, logger): pressed = 1 fd = self.dev.get_fd() p = poll() p.register(fd, POLLIN) evt = xwiimote.event() while True: p.poll() try: self.dev.dispatch(evt) code, state = evt.get_key() if evt.type == xwiimote.EVENT_KEY and state == pressed: logger.debug(self.key_map.get(code)) elif evt.type == xwiimote.EVENT_ACCEL: x, y, z = evt.get_abs(0) logger.debug("({x}, {y}, {z})".format(x=x, y=y, z=z)) except IOError as e: if e.errno != errno.EAGAIN: logger.error(e)
dev.set_led(4, dev.get_led(4) == False) print "capacity:", dev.get_battery(), "%" print "devtype:", dev.get_devtype() print "extension:", dev.get_extension() except SystemError as e: print "ooops", e exit(1) dev.set_mp_normalization(10, 20, 30, 40) x, y, z, factor = dev.get_mp_normalization() print "mp", x, y, z, factor # read some values p = poll() p.register(fd, POLLIN) evt = xwiimote.event() n = 0 while n < 2: p.poll() try: dev.dispatch(evt) if evt.type == xwiimote.EVENT_KEY: code, state = evt.get_key() print "Key:", code, ", State:", state n += 1 elif evt.type == xwiimote.EVENT_GONE: print "Gone" n = 2 elif evt.type == xwiimote.EVENT_WATCH: print "Watch" elif evt.type == xwiimote.EVENT_CLASSIC_CONTROLLER_KEY:
def leerSensores(mdevice, p1, numlecturas): calibration = [] valormin = 15000 valorMax = 0 for i in xrange(4): calibration.append(0) leidos = numlecturas event = xwiimote.event() #for i in xrange(numlecturas): while leidos > 0: #print str(i) p1.poll() # blocks #event = xwiimote.event() try: mdevice.dispatch(event) if event.type == xwiimote.EVENT_KEY: code, state = event.get_key() print "Key:", code, ", State:", state elif event.type == xwiimote.EVENT_GONE: print "Gone" elif event.type == xwiimote.EVENT_WATCH: print "Watch" elif event.type == xwiimote.EVENT_CLASSIC_CONTROLLER_KEY: code, state = event.get_key() print "Classical controller key:", code, state tv_sec, tv_usec = event.get_time() print tv_sec, tv_usec event.set_key(xwiimote.KEY_HOME, 1) code, state = event.get_key() print "Classical controller key:", code, state event.set_time(0, 0) tv_sec, tv_usec = event.get_time() print tv_sec, tv_usec elif event.type == xwiimote.EVENT_CLASSIC_CONTROLLER_MOVE: x, y, z = event.get_abs(0) print "Classical controller move 1:", x, y event.set_abs(0, 1, 2, 3) x, y, z = event.get_abs(0) print "Classical controller move 1:", x, y x, y, z = event.get_abs(1) print "Classical controller move 2:", x, y elif event.type == xwiimote.EVENT_IR: for i in [0, 1, 2, 3]: if event.ir_is_valid(i): x, y, z = event.get_abs(i) print "IR", i, x, y, z #else: # if event.type != xwiimote.EVENT_ACCEL: # print "type:", event.type else: leidos -= 1 # Type 3 # print "type:", event.type calibration[0] += event.get_abs(0)[0] calibration[3] += event.get_abs(1)[0] calibration[2] += event.get_abs(2)[0] calibration[1] += event.get_abs(3)[0] temporal = 0 #calculamos el peso instantaneo para sacar el max/min for z in xrange(4): temporal += event.get_abs(z)[0] if temporal > valorMax: valorMax = temporal elif temporal < valormin: valormin = temporal #event.set_abs(0, 0, 0, 0) #event.set_abs(1, 0, 0, 0) #event.set_abs(2, 0, 0, 0) #event.set_abs(3, 0, 0, 0) except IOError as e: if e.errno != errno.EAGAIN: print "Bad" calibration[0] = calibration[0] / numlecturas calibration[3] = calibration[3] / numlecturas calibration[2] = calibration[2] / numlecturas calibration[1] = calibration[1] / numlecturas named_calibration = { 'right_top': calibration[0], 'right_bottom': calibration[3], 'left_top': calibration[2], 'left_bottom': calibration[1], } #print(calibration) #print(named_calibration) #print("Max " + str(valorMax) + " min " + str(valormin)) #print("Dif " + str(valorMax - valormin)) #print("terminada lectura") return named_calibration, valorMax, valormin