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