def __init__(self, devpath):
super().__init__()
self.dev = xwiimote.iface(devpath)
self.dev.open(xwiimote.IFACE_BALANCE_BOARD)
self.p = select.epoll.fromfd(self.dev.get_fd())
self.m = None
self.run_flag = True
def __init__(self):
device = self._wait_for_balanceboard()
if device is None:
raise Exception("Found non-WiiBalanceBoard device")
self.iface = xwiimote.iface(device)
self.iface.open(xwiimote.IFACE_BALANCE_BOARD)
self.init_pool = False
def connect_balanceboard():
global bbaddress
#device is something like "/sys/devices/platform/soc/3f201000.uart/tty/ttyAMA0/hci0/hci0:11/0005:057E:0306.000C"
device = wait_for_balanceboard()
iface = xwiimote.iface(device)
iface.open(xwiimote.IFACE_BALANCE_BOARD)
(kg, err) = average_mesurements(measurements(iface))
#
# do something with this data
# like log to file or send to server
#
print("{:.2f} +/- {:.2f}".format(kg / 100.0 + calibration, err /
100.0)) #add adjustment to final result
print("publish to MQTT")
client.publish("mqtt/topic",
"{:.2f}".format(kg / 100.0 + calibration),
retain=True) #publish to topic
# find address of the balance board (once) and disconnect (if found).
if bbaddress is None:
bbaddress = find_device_address()
if bbaddress is not None:
device = bluezutils.find_device(bbaddress)
device.Disconnect()
def __init__(self):
device = self._wait_for_balanceboard()
if device is None:
raise Exception("Found non-WiiBalanceBoard device")
self.iface = xwiimote.iface(device)
self.iface.open(xwiimote.IFACE_BALANCE_BOARD)
self.init_pool = False
def main():
if len(sys.argv) == 2:
device = sys.argv[1]
else:
device = wait_for_balanceboard()
iface = xwiimote.iface(device)
iface.open(xwiimote.IFACE_BALANCE_BOARD)
try:
# for m in measurements(iface):
# print_bboard_measurements(*m)
for kg, err in average_mesurements(measurements(iface)):
pkg = "qme.seri.wiiweight.weight"
perr = "qme.seri.wiiweight.err"
print("{:.2f} +/- {:.2f}".format(kg / 100.0, err / 100.0))
kg, err = (int(round(x, 0)) for x in (kg, err))
for d, p in zip((kg, err), (pkg, perr)):
print(d, p)
util.submit(p, d)
sys.exit(0)
except KeyboardInterrupt:
print("Bye!")
def main():
if len(sys.argv) == 2:
device = sys.argv[1]
else:
device = wait_for_balanceboard()
iface = xwiimote.iface(device)
iface.open(xwiimote.IFACE_BALANCE_BOARD)
try:
# for m in measurements(iface):
# print_bboard_measurements(*m)
for kg, err in average_mesurements(measurements(iface)):
pkg = "qme.seri.wiiweight.weight"
perr = "qme.seri.wiiweight.err"
print("{:.2f} +/- {:.2f}".format(kg/100.0, err/100.0))
kg, err = (int(round(x, 0)) for x in (kg, err))
for d, p in zip((kg, err), (pkg, perr)):
print(d, p)
util.submit(p, d)
sys.exit(0)
except KeyboardInterrupt:
print("Bye!")
def initBoard(self):
#global xwiimote
global iface
global p
if len(sys.argv) == 2:
wiimote = sys.argv[1]
else:
wiimote = balanceboard.wait_for_balanceboard()
iface = xwiimote.iface(wiimote)
iface.open(xwiimote.IFACE_BALANCE_BOARD)
p = select.epoll.fromfd(iface.get_fd())
self.bateria = balanceboard.stado_inicial(iface, p)
self.lock = threading.Lock()
self.calibrarBB(iface, p)
self.named_wii = self.named_calibration
self.xpos = 0
self.ypos = 0
self.timer = QtCore.QBasicTimer()
self.weight = 0
self.weightText = ""
self.pararHilo = True
self.setFocusPolicy(QtCore.Qt.StrongFocus)
self.start()
self.lanzarHilo()
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 initBoard(self):
#global xwiimote
global iface
global p
if len(sys.argv) == 2:
wiimote = sys.argv[1]
else:
wiimote = balanceboard.wait_for_balanceboard()
iface = xwiimote.iface(wiimote)
iface.open(xwiimote.IFACE_BALANCE_BOARD)
p = select.epoll.fromfd(iface.get_fd())
self.bateria = balanceboard.stado_inicial(iface, p)
self.lock = threading.Lock()
self.calibrarBB(iface, p)
self.named_wii = self.named_calibration
self.xpos = 0
self.ypos = 0
self.timer = QtCore.QBasicTimer()
self.weight = 0
self.weightText = ""
self.pararHilo = True
self.setFocusPolicy(QtCore.Qt.StrongFocus)
self.start()
self.lanzarHilo()
def get_iface(ent, logger):
dev = None
try:
dev = xwiimote.iface(ent)
except IOError as e:
logger.error("Unable to create interface: ", e)
return dev
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 read_monitor(mon, wiimotes):
newmotes = []
wiimote_path = mon.poll()
while wiimote_path is not None:
try:
dev = xwiimote.iface(wiimote_path)
wiimotes['all'].append(dev)
news = open_subdevices(dev, wiimotes)
newmotes.append(dev)
except IOError as eo:
print "Fail on creating the wiimote (", eo, ")"
wiimote_path = mon.poll()
return newmotes
def read_monitor(mon, wiimotes):
newmotes = []
wiimote_path = mon.poll()
while wiimote_path is not None:
try:
dev = xwiimote.iface(wiimote_path)
wiimotes['all'].append(dev)
news = open_subdevices(dev, wiimotes)
newmotes.append(dev)
except IOError as eo:
print "Fail on creating the wiimote (", eo, ")"
wiimote_path = mon.poll()
return newmotes
def get_balance_board_iface():
mon = xwiimote.monitor(True, True)
wiimote_path = mon.poll()
while wiimote_path is not None:
dev = xwiimote.iface(wiimote_path)
if dev.get_extension() == 'balanceboard':
mask = dev.available() | xwiimote.IFACE_BALANCE_BOARD
dev.open(mask)
yield dev
dev.close()
break
wiimote_path = mon.poll()
else:
raise BalanceBoardConnectionError
def setup_wiimote(self, wiimote):
try:
# create new iface
dev = xwiimote.iface(wiimote)
# open device
sleep(0.1)
dev.open(dev.available() | xwiimote.IFACE_WRITABLE)
print("Opened device")
# register it with the poller
self.poller.register(dev.get_fd(), POLLIN)
return dev
except IOError as e:
print("ooops,", e)
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 __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 calibrarBB( self, iface, p ):
iface.close(0)
wiimote = balanceboard.wait_for_balanceboard()
iface = xwiimote.iface(wiimote)
#print("syspath:" + iface.get_syspath())
fd = iface.get_fd()
print("fd:", fd)
print("opened mask:", iface.opened())
iface.open(xwiimote.IFACE_BALANCE_BOARD)
self.bateria = balanceboard.stado_inicial(iface, p)
maximo = 15000
minimo = 0
self.lock.acquire()
while (maximo - minimo) > 20:
self.named_calibration, maximo, minimo = balanceboard.leerSensores(iface, p, Board.NumCalibraciones)
print(self.named_calibration)
print("Max " + str(maximo) + " min " + str(minimo))
print("Dif " + str(maximo - minimo))
self.lock.release()
def calibrarBB(self, iface, p):
iface.close(0)
wiimote = balanceboard.wait_for_balanceboard()
iface = xwiimote.iface(wiimote)
#print("syspath:" + iface.get_syspath())
fd = iface.get_fd()
print("fd:", fd)
print("opened mask:", iface.opened())
iface.open(xwiimote.IFACE_BALANCE_BOARD)
self.bateria = balanceboard.stado_inicial(iface, p)
maximo = 15000
minimo = 0
self.lock.acquire()
while (maximo - minimo) > 20:
self.named_calibration, maximo, minimo = balanceboard.leerSensores(
iface, p, Board.NumCalibraciones)
print(self.named_calibration)
print("Max " + str(maximo) + " min " + str(minimo))
print("Dif " + str(maximo - minimo))
self.lock.release()
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 connect_balanceboard():
global bbaddress
#device is something like "/sys/devices/platform/soc/3f201000.uart/tty/ttyAMA0/hci0/hci0:11/0005:057E:0306.000C"
device = wait_for_balanceboard()
iface = xwiimote.iface(device)
iface.open(xwiimote.IFACE_BALANCE_BOARD)
(kg, err) = average_mesurements(measurements(iface))
#
# do something with this data
# like log to file or send to server
#
print("{:.2f} +/- {:.2f}".format(kg/100.0, err/100.0))
# find address of the balance board (once) and disconnect (if found).
if bbaddress is None:
bbaddress = find_device_address()
if bbaddress is not None:
device = bluezutils.find_device(bbaddress)
device.Disconnect()
ent = mon.poll()
firstwiimote = ent
while ent is not None:
print "Found device: " + ent
ent = mon.poll()
except SystemError as e:
print "ooops, cannot create monitor (", e, ")"
# continue only if there is a wiimote
if firstwiimote is None:
print "No wiimote to read"
exit(0)
# create a new iface
try:
dev = xwiimote.iface(firstwiimote)
except IOError as e:
print "ooops,", e
exit(1)
# display some information and open the iface
try:
print "syspath:" + dev.get_syspath()
fd = dev.get_fd()
print "fd:", fd
print "opened mask:", dev.opened()
dev.open(dev.available() | xwiimote.IFACE_WRITABLE)
print "opened mask:", dev.opened()
dev.rumble(True)
sleep(1 / 4.0)
def _device_is_balanceboard(self, device):
time.sleep(2)
iface = xwiimote.iface(device)
return iface.get_devtype() == 'balanceboard'
def dev_is_balanceboard(devpath):
time.sleep(0.5) # too early check is reported as 'unknown'
iface = xwiimote.iface(devpath)
return iface.get_devtype() == 'balanceboard'
def InitBoard():
iface = xwiimote.iface(WaitForBoard())
iface.open(xwiimote.IFACE_BALANCE_BOARD)
board_p = select.epoll.fromfd(iface.get_fd())
return iface, board_p
def dev_is_balanceboard(dev):
time.sleep(
2) # if we check the devtype to early it is reported as 'unknown' :(
iface = xwiimote.iface(dev)
return iface.get_devtype() == 'balanceboard'
def dev_is_balanceboard(dev):
time.sleep(2) # if we check the devtype to early it is reported as 'unknown' :(
iface = xwiimote.iface(dev)
return iface.get_devtype() == 'balanceboard'
def _device_is_balanceboard(self,device):
time.sleep(2)
iface = xwiimote.iface(device)
return iface.get_devtype() == 'balanceboard'
