| [4] | 1 | from managerConn import ManagerConnection |
|---|
| 2 | import time, sys |
|---|
| 3 | from pywii import Wii, _ClassWiimote |
|---|
| 4 | import pywii |
|---|
| 5 | import cmath |
|---|
| 6 | |
|---|
| 7 | # types of messages sent to the manager |
|---|
| 8 | MOVE = 0 |
|---|
| 9 | TRANSLATE = 1 |
|---|
| 10 | ROTATE = 2 |
|---|
| 11 | RESIZE = 3 |
|---|
| 12 | CHANGE_Z = 4 |
|---|
| 13 | FULLSCREEN = 5 |
|---|
| 14 | |
|---|
| 15 | |
|---|
| 16 | # wiimote Buttons |
|---|
| 17 | BUTTON_A = 0 |
|---|
| 18 | BUTTON_B = 1 |
|---|
| 19 | BUTTON_1 = 2 |
|---|
| 20 | BUTTON_2 = 3 |
|---|
| 21 | BUTTON_PLUS = 4 |
|---|
| 22 | BUTTON_MINUS = 5 |
|---|
| 23 | BUTTON_HOME = 6 |
|---|
| 24 | BUTTON_UP = 7 |
|---|
| 25 | BUTTON_DOWN = 8 |
|---|
| 26 | BUTTON_LEFT = 9 |
|---|
| 27 | BUTTON_RIGHT = 10 |
|---|
| 28 | |
|---|
| 29 | # constants for gaussaian smoothing |
|---|
| 30 | HISTORY_LENGTH = 150 |
|---|
| 31 | STD_DEVIATION = 15.0 |
|---|
| 32 | |
|---|
| 33 | #Rotating constants |
|---|
| 34 | NO_ROTATE = 0 |
|---|
| 35 | LEFT_ROTATE = 1 |
|---|
| 36 | RIGHT_ROTATE = -1 |
|---|
| 37 | |
|---|
| 38 | class Position: |
|---|
| 39 | def __init__(self, x, y): |
|---|
| 40 | self.x = x |
|---|
| 41 | self.y = y |
|---|
| 42 | |
|---|
| 43 | |
|---|
| 44 | class 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 |
|---|
| 233 | port = 20005 |
|---|
| 234 | if len(sys.argv) < 2: |
|---|
| 235 | print "Usage: python " + sys.argv[0] + " sageIP" |
|---|
| 236 | sys.exit(0) |
|---|
| 237 | elif len(sys.argv) > 2: |
|---|
| 238 | port = int(sys.argv[2]) |
|---|
| 239 | |
|---|
| 240 | # start everything off |
|---|
| 241 | Wiimote( ManagerConnection(sys.argv[1], port) ) |
|---|
| 242 | |
|---|
| 243 | |
|---|
