import sys import PyQt4 from PyQt4 import QtCore from PyQt4 import QtGui from PyQt4.Qt import QThread from wimot import * import random from threading import Thread as QThread import cwiid import time import datetime import os, math import os.path import random import shm, struct os.chdir('/tmp') #from hellogl import GLWidget import wiicount5 as wiicount led = [ 0x0, #0x1|0x8, 0x0, #0x1|0x8, ] WIIMOTES=[ '00:19:1D:C3:D2:B4', # nowy '00:19:1D:AA:F9:20', # stary ] def fifo_write(fd, msg): fd.write('%04i' % len(msg)) fd.write(msg) fd.flush() def fifo_read(fd): a = int(fd.read(4)) return fd.read(a) class MyForm(QtGui.QMainWindow): points1 = [] points2 = [] battery1 = False battery2 = False thepoint = [[],[]] pointno = 1 lines = [] st = '' cs = None t0 = 0 reg = [0,0,0,0,0,0] reg_cter = 0 duel_cter = 0 counter = 0 def __init__(self, parent=None): QtGui.QWidget.__init__(self, parent) self.ui = Ui_Dialog() self.ui.setupUi(self) for box in [self.ui.irdabox1, self.ui.irdabox2, self.ui.thebox]: scene = QtGui.QGraphicsScene() box.setScene(scene) size = box.maximumViewportSize() box.pm = QtGui.QPixmap( size.width(),size.height() ) box.pm.fill() box.paint = QtGui.QPainter() box.scene = scene scene.addPixmap(box.pm) QtCore.QObject.connect(self, QtCore.SIGNAL("view_points"), self.view_points) QtCore.QObject.connect(self, QtCore.SIGNAL("battery_status"), self.battery_status) self.state_check('start') def remove_odbicie(self, points, nr): return points points = sorted(points, key=lambda a:a[1]) # od najwyzszego a,b,a_ = points[0] for (x,y,x_) in points[1:]: d = math.sqrt((a-x)**2 + (b-y)**2) if d < 0.1: return [(a,b,a_), (x,y,x_)] #print "!!! %r" % points a = sorted(points, key=lambda a:a[1]) # od najwyzszego if len(points) == 2: if abs(a[0][0] - a[1][0]) < 0.1: return [a[0]] if len(points) == 3: if (abs(a[0][0] - a[1][0]) < 0.1) and (abs(a[0][0] - a[2][0]) < 0.1): return [a[0]] return points def state_check(self, st): if st == 'start': self.view_points(0, ".. not connected ..") self.view_points(1, ".. not connected ..") self.ui.msg1.setText('.. not connected ..') self.ui.msg2.setText('.. not connected ..') return if st.startswith('battery'): if st == 'battery1': self.battery1 = True if st == 'battery2': self.battery2 = True if self.battery1 and self.battery2: self.state_check('noise') return if st =='data' and self.st == '': return if st == 'noise': self.ui.msg1.setText('listening for static noise') self.ui.msg2.setText('listening for static noise') self.t0 = time.time() self.st = 'noise' return assert st == 'data' if self.st == 'noise': if self.points1 or self.points2: if self.points1: self.ui.msg1.setText('noise...') if self.points2: self.ui.msg2.setText('noise...') self.t0 = time.time() return else: d = time.time() - self.t0 if d > 2: self.ui.msg1.setText('listening for static noise') self.ui.msg2.setText('listening for static noise') if d > 3: self.st = 'point' #led[0] = 0x4 | 0x2 | 0x1 | 0x8 #led[1] = 0x4 | 0x2 | 0x1 | 0x8 self.t0 = None return c = 1 if self.st == 'point' else 2 if len(self.points1) > c: self.points1 = self.remove_odbicie(self.points1,1) if len(self.points2) > c: self.points2 = self.remove_odbicie(self.points2,2) if len(self.points1) > c: self.ui.msg1.setText('noise!') self.t0 = time.time() return if len(self.points2) > c: self.ui.msg2.setText('noise!') self.t0 = time.time() return if self.t0 is not None: if (time.time() - self.t0) > 2: self.t0 = None else: return if self.st == 'point': if len(sys.argv) > 1: self.thepoint = eval(sys.argv[1]) self.pointno = 7 self.st = 'data' self.cs = wiicount.Cameras( [self.thepoint[0], self.thepoint[1]] ) self.ui.msg1.setText('') self.ui.msg2.setText('') return if len(self.thepoint[0]) < self.pointno: self.ui.msg1.setText('press point %i' % self.pointno ) self.ui.msg2.setText('press point %i' % self.pointno ) if self.points1: self.ui.msg1.setText('ok') if self.points2: self.ui.msg2.setText('ok') if self.points1 and self.points2: #print 'asd %r' % self.pointno self.thepoint[0].append( self.points1[0] ) self.thepoint[1].append( self.points2[0] ) else: self.ui.msg1.setText('ok') self.ui.msg2.setText('ok') if not self.points1 and not self.points2: self.t0 = time.time() - 1.0 #led[0] = led[0] & ~( 1 << (self.pointno-1)) #led[1] = led[1] & ~( 1 << (self.pointno-1)) print self.pointno self.pointno += 1 if self.pointno > 6: self.st = 'data' print "thepoint=%r" % self.thepoint self.cs = wiicount.Cameras( [self.thepoint[0], self.thepoint[1]] ) self.ui.msg1.setText('') self.ui.msg2.setText('') return assert(self.st == 'data') if len(self.points1)==0 or len(self.points2)==0: if self.reg_cter != -1: self.reg_cter += 1 if self.reg_cter > 4: wiicount.move_update(self.cs, None) self.reg_cter = -1 return self.reg_cter = 0 if len(self.points1)!=2 or len(self.points2)!=2: return a = wiicount.move_update(self.cs, [map(lambda (x,y,_):(x,y),self.points1), map(lambda (x,y,_):(x,y),self.points2)]) if a: x,y,z = a self.view_xy(x,y,z) def battery_status(self, no, bat): if no == 0: box = self.ui.battery1 if not self.battery1: self.state_check('battery1') elif no == 1: box = self.ui.battery2 if not self.battery2: self.state_check('battery2') box.setText('battery: %1.0f%%' % bat) last_time = None def view_points(self, no, data): if no == 0: box = self.ui.irdabox1 if not isinstance(data,str): self.points1 = data elif no == 1: box = self.ui.irdabox2 if not isinstance(data,str): self.points2 = data if not self.last_time: self.last_time = time.time() if time.time() - self.last_time > (1.0/40): self.last_time = time.time() if isinstance(data, str) or isinstance(data, unicode): paint = box.paint box.pm.fill() paint.begin(box.pm) paint.setRenderHint(QtGui.QPainter.TextAntialiasing, True) paint.setPen(QtCore.Qt.black) paint.drawText(paint.viewport(), QtCore.Qt.AlignCenter, data) paint.end() box.scene.removeItem( box.scene.items()[0] ) box.scene.addPixmap(box.pm) return else: box.pm.fill() box.paint.begin(box.pm) box.paint.setRenderHint(QtGui.QPainter.Antialiasing, True) box.paint.setPen(QtCore.Qt.black) box.paint.setBrush(QtCore.Qt.SolidPattern) for x, y, siz in data: size = box.pm.size() x = x * size.width() y = y * size.height() siz *= 2 siz = max(siz, 2) box.paint.drawEllipse(x-siz/2, y-siz/2, siz, siz) box.paint.setPen(QtCore.Qt.blue) for x, y, siz in self.thepoint[no]: size = box.pm.size() x = x * size.width() y = y * size.height() siz *= 2 siz = max(siz, 2) box.paint.drawEllipse(x-siz/2, y-siz/2, siz, siz) """ if self.cs and self.cs.cameras: c = self.cs.cameras[no] if box.paint.setPen(QtCore.Qt.red) x, y = c.midpoint x = x * size.width() y = y * size.height() box.paint.drawEllipse(x-3/2, y-3/2, 3, 3) """ box.paint.end() box.scene.removeItem( box.scene.items()[0] ) box.scene.addPixmap(box.pm) self.state_check('data') last_time_xy = None def view_xy(self, x,y,z): if not self.last_time_xy: self.last_time_xy = time.time() if time.time() - self.last_time_xy < (1.0/10): return self.last_time_xy = time.time() box = self.ui.thebox box.pm.fill() box.paint.begin(box.pm) box.paint.setRenderHint(QtGui.QPainter.Antialiasing, True) box.paint.setPen(QtCore.Qt.red) box.paint.setBrush(QtCore.Qt.SolidPattern) size = box.pm.size() x = (x+0.5) * size.width() y = (-y+0.5) * size.height() z = z * 10.0 siz = max(1, z) box.paint.drawEllipse(x-siz/2, y-siz/2, siz, siz) box.paint.drawEllipse(x-siz/2, y-siz/2, siz, siz) box.paint.setPen(QtCore.Qt.blue) for a,b in [(0.25,0.25),(0.25,0.5),(0.5,0.25),(0.75,0.5),(0.5,0.75),(0.75,0.25),(0.25,0.75),(0.75,0.75),(0.5,0.5),]: x = a * size.width() y = b * size.height() siz = 1.0 box.paint.drawEllipse(x-siz/2, y-siz/2, siz, siz) box.paint.end() box.scene.removeItem( box.scene.items()[0] ) box.scene.addPixmap(box.pm) class WiimoteThread(QThread): wm_id = None bdaddr = None '''['__class__', '__delattr__', '__doc__', '__getattribute__', '__hash__', '__init__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__str__', 'close', 'disable', 'enable', 'get_acc_cal', 'get_mesg', 'led', 'mesg_callback', 'read', 'request_status', 'rpt_mode', 'rumble', 'state', 'write'] ['led', 'rpt_mode', 'ext_type', 'rumble', 'error', 'battery'] ''' def run(self): fifoq = 'wii-%s-query' % self.bdaddr.replace(':','') fifor = 'wii-%s-resp' % self.bdaddr.replace(':','') if not os.path.exists(fifoq): os.mkfifo(fifoq) if not os.path.exists(fifor): os.mkfifo(fifor) for i in range(10): try: fdq = open(fifoq, 'w') fdr = open(fifor, 'r') break except IOError: time.sleep(1.2) fdq = None fdr = None if not fdr or not fdq: print "run wii daemon" sys.exit(0) lastled = 0x0 fifo_write(fdq, '%r' % 'battery') bat = eval( fifo_read(fdr) ) myapp.emit(QtCore.SIGNAL("battery_status"), self.wm_id, bat) while True: if led[self.wm_id] != lastled: fifo_write(fdq, repr( ('led', led[self.wm_id]), )) eval( fifo_read(fdr) ) lastled = led[self.wm_id] fifo_write(fdq, '%r' % 'irda') data = eval( fifo_read(fdr) ) myapp.emit(QtCore.SIGNAL("view_points"),self.wm_id, data) time.sleep(0.0001) ''' // == -222... +222; 800 shm[0] = SCALE((-200 MM), (1800 MM), -230, 230, sp.position[0]); // || -111 ... +90; 700 shm[1] = SCALE((300 MM), (1100 MM), -120, 130, sp.position[2]); // UP -95 ... +190 shm[2] = SCALE((-300 MM), (2000 MM), -160, 200, -sp.position[1]); ''' # 885 0 470 180 0 0 -> base/robot # 867 712 -139 0 0 90 -> user frame 1 if __name__ == '__main__': try: mem = shm.create_memory(0xDEAD, 1*1024, 0666) except shm.error: mem = shm.memory( shm.getshmid(0xDEAD) ) mem.attach() wiicount.setmem(mem) app = QtGui.QApplication(sys.argv) myapp = MyForm() myapp.show() for i, bdaddr in enumerate(WIIMOTES): t = WiimoteThread() t.wm_id = i t.bdaddr = bdaddr t.start() sys.exit(app.exec_())