source: trunk/src/testing/dim/hwcapture/wiimote.py @ 4

Revision 4, 8.0 KB checked in by ajaworski, 13 years ago (diff)

Added modified SAGE sources
Line 
1from managerConn import ManagerConnection
2import time, sys
3from pywii import Wii, _ClassWiimote
4import pywii
5import cmath
6
7# types of messages sent to the manager
8MOVE = 0
9TRANSLATE = 1
10ROTATE = 2
11RESIZE = 3
12CHANGE_Z = 4
13FULLSCREEN = 5
14
15
16# wiimote Buttons
17BUTTON_A = 0
18BUTTON_B = 1
19BUTTON_1 = 2
20BUTTON_2 = 3
21BUTTON_PLUS = 4
22BUTTON_MINUS = 5
23BUTTON_HOME = 6
24BUTTON_UP = 7
25BUTTON_DOWN = 8
26BUTTON_LEFT = 9
27BUTTON_RIGHT = 10
28
29# constants for gaussaian smoothing
30HISTORY_LENGTH = 150
31STD_DEVIATION = 15.0
32
33#Rotating constants
34NO_ROTATE = 0
35LEFT_ROTATE = 1
36RIGHT_ROTATE = -1
37
38class Position:
39    def __init__(self, x, y):
40        self.x = x
41        self.y = y
42   
43
44class Wiimote:
45    def __init__(self, manager):
46        self.__buttons = {}
47        for i in range (0, 11):
48            self.__buttons[i] = False
49           
50        self.history = []       
51        self.gauss = []
52        for i in range (-HISTORY_LENGTH + 1, 1):
53            self.gauss.append(abs(1/(cmath.sqrt(2*cmath.pi*pow(STD_DEVIATION,2)))*cmath.exp(-(pow(i,2)/(2*pow(STD_DEVIATION,2))))))
54       
55        gaussSum = 0       
56        for i in self.gauss:
57            gaussSum += i
58           
59        for i in range(0,HISTORY_LENGTH):
60            self.gauss[i] = self.gauss[i] / gaussSum
61           
62        self.pointerPosition = None
63       
64        self.wii = Wii()
65        self.wiimote = self.wii.QuickConnect()[0]
66        self.wiimote.SetMotionSensingMode(self.wiimote.ON)
67        self.wiimote.SetLEDs(_ClassWiimote.LED_1)
68        self.wiimote.IR.SetMode(self.wiimote.IR.ON)
69        self.wiimote.IR.SetAspectRatio(self.wiimote.IR.ASPECT_16_9)
70       
71        self.initialOrientation = [0,0]
72        self.displaySize = [660.0, 370.0]
73       
74        self.outPointerCount = 0
75       
76#        Potrzebne przy obrotach na akcelerometrze
77#        self.rotateInit = 80
78#        self.rotateDone = 40
79#        self.rotating = NO_ROTATE
80
81        self.buttonMap = {}
82       
83        self.buttonMap[BUTTON_A] = self.wiimote.Buttons.BUTTON_A
84        self.buttonMap[BUTTON_B] = self.wiimote.Buttons.BUTTON_B
85        self.buttonMap[BUTTON_1] = self.wiimote.Buttons.BUTTON_ONE
86        self.buttonMap[BUTTON_2] = self.wiimote.Buttons.BUTTON_TWO
87        self.buttonMap[BUTTON_PLUS] = self.wiimote.Buttons.BUTTON_PLUS
88        self.buttonMap[BUTTON_MINUS] = self.wiimote.Buttons.BUTTON_MINUS
89        self.buttonMap[BUTTON_HOME] = self.wiimote.Buttons.BUTTON_HOME
90        self.buttonMap[BUTTON_UP] = self.wiimote.Buttons.BUTTON_UP
91        self.buttonMap[BUTTON_DOWN] = self.wiimote.Buttons.BUTTON_DOWN
92        self.buttonMap[BUTTON_LEFT] = self.wiimote.Buttons.BUTTON_LEFT
93        self.buttonMap[BUTTON_RIGHT] = self.wiimote.Buttons.BUTTON_RIGHT
94       
95        self.buttonHandler = {}
96       
97        self.__registerButtonHandler(BUTTON_A, self.onButtonA)
98        self.__registerButtonHandler(BUTTON_B, self.onButtonB)
99        self.__registerButtonHandler(BUTTON_LEFT, self.onButtonLeft)
100        self.__registerButtonHandler(BUTTON_RIGHT, self.onButtonRight)
101        self.__registerButtonHandler(BUTTON_UP, self.onButtonUp)
102        self.__registerButtonHandler(BUTTON_DOWN, self.onButtonDown)
103        self.__registerButtonHandler(BUTTON_HOME, self.onButtonHome)
104 
105        self.manager = manager
106       
107        handlerMap = {  self.wiimote.EVENT_EVENT: self.handleEvent }
108        try:
109            self.wii.PollMapped(handlerMap, self.wii.POLL_FOREVER)
110        except:
111            self.manager.quit()
112            sys.exit(0)
113       
114       
115       
116    def __registerButtonHandler(self, buttonCode, buttonHandler):
117        self.buttonHandler[buttonCode] = buttonHandler
118           
119       
120    def __calculateGaussAverage(self):
121        sumx = 0
122        sumy = 0
123       
124        sumweight = 0
125       
126        for i in range(0,len(self.history)):
127            sumx += self.history[i].x * self.gauss[i]
128            sumy += self.history[i].y * self.gauss[i]
129            sumweight += self.gauss[i]
130       
131        if len(self.history) < HISTORY_LENGTH:
132            self.pointerPosition = Position(sumx/sumweight, sumy/sumweight)
133        else:
134            self.pointerPosition = Position(sumx, sumy)
135           
136    def __updatePointerPosition(self, point):
137        if point.x == 0 and point.y == 1:
138            if self.outPointerCount == HISTORY_LENGTH / 3:
139                self.history = []
140                self.pointerPosition = Position(-1,-1)
141            else:
142                self.outPointerCount += 1
143           
144        else:
145            self.history.append(point)
146            self.pointerPosition = None
147           
148            self.outPointerCount = 0
149           
150            if len(self.history) == (HISTORY_LENGTH + 1):
151                self.history.pop(0)
152            self.__calculateGaussAverage()
153           
154    def handleEvent(self, wm):
155        for i in self.buttonHandler:
156            self.__checkSingleButton(wm, i)
157       
158#obroty na akcelerometrze
159#        if wm.Buttons.isPressed(wm.Buttons.BUTTON_A):
160#            pitch, roll, _ = wm.Accelerometer.GetOrientation()
161#            if self.rotating == NO_ROTATE:
162#                if self.initialOrientation[1] - roll > self.rotateInit:
163#                    self.rotating = LEFT_ROTATE
164#                    self.sendMsg(ROTATE,self.rotating)
165#                elif roll - self.initialOrientation[1]> self.rotateInit:
166#                    self.rotating = RIGHT_ROTATE
167#                    self.sendMsg(ROTATE,self.rotating)
168#            elif self.rotating == LEFT_ROTATE and self.initialOrientation[1] - roll < self.rotateDone:
169#                self.rotating = NO_ROTATE
170#                self.sendMsg(ROTATE,self.rotating)
171#            elif self.rotating == RIGHT_ROTATE and roll - self.initialOrientation[1] < self.rotateDone:
172#                self.rotating = NO_ROTATE
173#                self.sendMsg(ROTATE,self.rotating)
174               
175        if wm.isUsingIR() and wm.IR.GetNumDots >= 2:         
176            mx, my = wm.IR.GetCursorPosition()
177            x = float(mx/self.displaySize[0])
178            y = 1-float(my/self.displaySize[1])
179            self.__updatePointerPosition(Position(x,y))
180            if not self.pointerPosition == None:
181                self.sendMsg(MOVE,self.pointerPosition.x, self.pointerPosition.y)
182               
183    def __checkSingleButton(self, wm, buttonCode):
184        if wm.Buttons.isJustPressed(self.buttonMap[buttonCode]):
185            self.__buttons[buttonCode] = True
186            self.buttonHandler[buttonCode](wm, True)           
187           
188        if self.__buttons[buttonCode] and not wm.Buttons.isPressed(self.buttonMap[buttonCode]):
189            self.__buttons[buttonCode] = False
190            self.buttonHandler[buttonCode](wm, False)
191           
192           
193    def onButtonA(self, wm, isDown):
194        if isDown:
195            self.sendMsg(TRANSLATE, 1)
196            self.initialOrientation[0], self.initialOrientation[1], _ = wm.Accelerometer.GetOrientation()
197        else:
198            self.sendMsg(TRANSLATE, 0)
199   
200    def onButtonB(self, wm, isDown):
201        if isDown:
202            self.sendMsg(RESIZE, 1)
203        else:
204            self.sendMsg(RESIZE, 0)
205   
206    def onButtonLeft(self, wm, isDown):
207        if isDown:
208            self.sendMsg(ROTATE, LEFT_ROTATE)
209   
210    def onButtonRight(self, wm, isDown):
211        if isDown:
212            self.sendMsg(ROTATE, RIGHT_ROTATE)
213           
214    def onButtonUp(self, wm, isDown):
215        if isDown:
216            self.sendMsg(CHANGE_Z, 1)
217           
218    def onButtonDown(self, wm, isDown):
219        if isDown:
220            self.sendMsg(CHANGE_Z, -1)
221   
222    def onButtonHome(self, wm, isDown):
223        if isDown:
224            self.sendMsg(FULLSCREEN)
225
226    def sendMsg(self, *data):
227        msg = ""
228        for m in data:
229            msg+=str(m)+" "
230        self.manager.sendMessage("wii", "wiimote", msg)           
231
232# you can optionally pass in a port number of the manager on command line
233port = 20005
234if len(sys.argv) < 2:
235    print "Usage: python " + sys.argv[0] + " sageIP"
236    sys.exit(0)
237elif len(sys.argv) > 2:
238    port = int(sys.argv[2])
239
240# start everything off
241Wiimote( ManagerConnection(sys.argv[1], port) )
242
243   
Note: See TracBrowser for help on using the repository browser.