$search
00001 ################################################################################ 00002 # 00003 # File: wiistate.py 00004 # RCS: $Header: $ 00005 # Description: Object representation of Wiimote's state 00006 # Author: Andreas Paepcke 00007 # Created: Thu Aug 13 11:01:34 2009 (Andreas Paepcke) paepcke@anw.willowgarage.com 00008 # Modified: Thu Jan 13 13:50:15 2011 (Andreas Paepcke) paepcke@bhb.willowgarage.com 00009 # Language: Python 00010 # Package: N/A 00011 # Status: Experimental (Do Not Distribute) 00012 # 00013 # 00014 # 00015 ################################################################################ 00016 # 00017 # Revisions: 00018 # 00019 # Thu Mar 18 10:56:09 2010 (David Lu) davidlu@wustl.edu 00020 # Added nunchuk state variables 00021 # Fri Oct 29 08:58:21 2010 (Miguel Angel Julian Aguilar, QBO Project) miguel.angel@thecorpora.com 00022 # Added classic controller state variables 00023 # Mon Nov 08 11:43:23 2010 (David Lu) davidlu@wustl.edu 00024 # Added calibration for nunchuk 00025 ################################################################################ 00026 00027 from wiimoteConstants import * 00028 from wiiutils import * 00029 import numpy as np 00030 00031 #---------------------------------------- 00032 # Class WIIState 00033 #--------------- 00034 00035 class WIIState(object): 00036 """Holds the state of a WIIRemote-plus. 00037 00038 The state is passed in and is as communicated 00039 by one message from the WII+ device. We unpack 00040 the information and place it into individual 00041 dictionaries for callers to grab. 00042 00043 Public instance variables: 00044 o time Time in fractional seconds since beginning of Epoch of when 00045 state was measured (Float). 00046 o ascTime Time when state was measured (Human-readable) 00047 o rumble True/False if wiimote vibration is on/off 00048 o angleRate A GyroReading instance containing gyro (a.k.a. angular rate) measurement 00049 o acc A WIIReading instance containing accelerometer measurement corrected by 00050 the calibration information that is stored in the Wiimote 00051 o accRaw A WIIReading instance containing accelerometer measurement uncorrected 00052 o buttons A dictionary for which buttons are being held down. That could be 00053 multiple buttons. Keys are: 00054 BTN_1, BTN_2, BTN_PLUS, BTN_MINUS, BTN_A, BTN_B, 00055 BTN_UP, BTN_DOWN, BTN_LEFT, BTN_RIGHT, BTN_HOME 00056 Values are 1/0 00057 o IRSources Dictionary with on/off values for which IR lights are 00058 being sensed. Keys are: 00059 IR1, IR2, IR3, IR4 00060 Values are 1/0 00061 o motionPlusPresent True if a gyro Motion+ is plugged into the Wiimote. Else False 00062 00063 o nunchukPresent True if nunchuk is plugged in. Else False 00064 o nunchukAccRaw A WIIReading instance with acceleromoter measurement from the nunchuk (raw values) 00065 o nunchukAcc The same, but zeroed using factory calibration 00066 o nunchukStickRaw A tuple with the two axes of the joystick on the nunchuk, raw readings 00067 o nunchukStick A tuple with the two axes of the joystick on the nunchuk, zeroed to be [-1, 1] 00068 o nunchukButtons A dictionary for which nunchuk buttons are down. Keys are BTN_C and BTN_Z 00069 00070 Public methods: 00071 o setAccelerometerCalibration Bias setting for accelerometer. This triplet is used to 00072 turn raw accelerometer values into calibrated values. 00073 o setGyroCalibration Bias setting for gyro. This triplet is used to 00074 turn raw gyro values into calibrated values. 00075 """ 00076 00077 _accCalibrationZero = None 00078 _gyroZeroReading = None 00079 _nunchukZeroReading = None 00080 _nunchukJoystickZero = None 00081 00082 #---------------------------------------- 00083 # __init__ 00084 #---------- 00085 00086 def __init__(self, state, theTime, theRumble, buttonStatus): 00087 """Unpack the given state, normalizing if normalizers are passed in.""" 00088 00089 self.time = theTime 00090 self.ascTime = `theTime` 00091 self.rumble = theRumble 00092 self.IRSources = [None, None, None, None] 00093 self.battery = None 00094 self.acc = None 00095 self.accRaw = None 00096 self.angleRate = None 00097 self.angleRate = None 00098 self.angleRageRaw = None 00099 self.motionPlusPresent = False 00100 self.buttons = {BTN_1: False, BTN_2: False, BTN_PLUS: False, 00101 BTN_MINUS: False, BTN_A: False, BTN_B: False, 00102 BTN_UP: False, BTN_DOWN: False, BTN_LEFT: False, 00103 BTN_RIGHT: False, BTN_HOME: False} 00104 self.nunchukPresent = False 00105 self.nunchukAccRaw = None 00106 self.nunchukAcc = None 00107 self.nunchukStick = None 00108 self.nunchukStickRaw = None 00109 self.nunchukButtons = {BTN_C: False, BTN_Z: False} 00110 00111 self.classicPresent = False 00112 self.classicStickLeft = None 00113 self.classicStickRight = None 00114 self.classicButtons = {CLASSIC_BTN_A: False, CLASSIC_BTN_B: False, 00115 CLASSIC_BTN_L: False, CLASSIC_BTN_R: False, 00116 CLASSIC_BTN_X: False, CLASSIC_BTN_Y: False, 00117 CLASSIC_BTN_ZL: False, CLASSIC_BTN_ZR: False, 00118 CLASSIC_BTN_PLUS: False, CLASSIC_BTN_MINUS: False, 00119 CLASSIC_BTN_UP: False, CLASSIC_BTN_DOWN: False, 00120 CLASSIC_BTN_LEFT: False, CLASSIC_BTN_RIGHT: False, 00121 CLASSIC_BTN_HOME: False} 00122 00123 # Handle buttons on the WII 00124 # A zero means no button is down. 00125 00126 if buttonStatus == 0: 00127 for key in self.buttons.keys(): 00128 self.buttons[key] = False 00129 continue 00130 00131 self.buttons[BTN_1] = (buttonStatus & BTN_1) > 0 00132 self.buttons[BTN_2] = (buttonStatus & BTN_2) > 0 00133 self.buttons[BTN_PLUS] = (buttonStatus & BTN_PLUS) > 0 00134 self.buttons[BTN_MINUS] = (buttonStatus & BTN_MINUS) > 0 00135 self.buttons[BTN_A] = (buttonStatus & BTN_A) > 0 00136 self.buttons[BTN_B] = (buttonStatus & BTN_B) > 0 00137 self.buttons[BTN_UP] = (buttonStatus & BTN_UP) > 0 00138 self.buttons[BTN_DOWN] = (buttonStatus & BTN_DOWN) > 0 00139 self.buttons[BTN_LEFT] = (buttonStatus & BTN_LEFT) > 0 00140 self.buttons[BTN_RIGHT] = (buttonStatus & BTN_RIGHT) > 0 00141 self.buttons[BTN_HOME] = (buttonStatus & BTN_HOME) > 0 00142 00143 for msgComp in state: 00144 # msgComp has: (1,2) for Status:Button one pushed, or 00145 # (3, [None,None,None,None]) for LEDs 00146 # (7, {'angle_rage': (123,456,789)}) 00147 msgType = msgComp[0] 00148 00149 if msgType == WII_MSG_TYPE_ACC: 00150 # Second list member is accelerator triplet of numbers: 00151 accStatus = msgComp[1] 00152 self.accRaw = WIIReading(accStatus, self.time) 00153 00154 # If this class knows about accelerometer calibration 00155 # data, correct the raw reading: 00156 if self._accCalibrationZero is not None and self._accCalibrationOne is not None: 00157 self.acc = WIIReading((self.accRaw - self._accCalibrationZero) / (self._accCalibrationOne - self._accCalibrationZero), self.time) 00158 else: 00159 self.acc = WIIReading(self.accRaw, self.time) 00160 00161 continue 00162 00163 elif msgType == WII_MSG_TYPE_IR: 00164 # Second list member is a list of 4 dictionaries, 00165 # one for each of the Wii IR sources: 00166 00167 IRStatus = msgComp[1] 00168 # IRStatus is an array of four Dicts. Ex: [{'pos': (317, 445)}, None, None, None] 00169 self.IRSources[0] = IRStatus[0] 00170 self.IRSources[1] = IRStatus[1] 00171 self.IRSources[2] = IRStatus[2] 00172 self.IRSources[3] = IRStatus[3] 00173 00174 continue 00175 00176 elif msgType == WII_MSG_TYPE_MOTIONPLUS: 00177 # Second list member is a dictionary with the single 00178 # key 'angle_rate', which yields as its value a gyro 00179 # readings triplet of numbers: 00180 00181 gyroDict = msgComp[1] 00182 00183 if gyroDict is not None: 00184 # If this class knows about a zero-movement reading for the 00185 # gyro, subtract that reading from the raw measurement: 00186 00187 self.angleRateRaw = GyroReading(gyroDict['angle_rate'], self.time) 00188 if self._gyroZeroReading is not None: 00189 self.angleRate = GyroReading(self.angleRateRaw - self._gyroZeroReading, self.time) 00190 else: 00191 self.angleRate = self.angleRateRaw 00192 00193 self.motionPlusPresent = True 00194 00195 continue 00196 elif msgType == WII_MSG_TYPE_NUNCHUK: 00197 nunChuk = msgComp[1]; 00198 if nunChuk is not None: 00199 self.nunchukPresent = True 00200 self.nunchukAccRaw = WIIReading(nunChuk['acc'], self.time) 00201 00202 # If this class knows about accelerometer calibration 00203 # data, correct the raw reading: 00204 if self._nunchukZeroReading is not None: 00205 self.nunchukAcc = WIIReading((self.nunchukAccRaw - self._nunchukZeroReading) / (self._nunchukOneReading - self._nunchukZeroReading), self.time) 00206 else: 00207 self.nunchukAcc = WIIReading(self.nunchukAccRaw, self.time) 00208 00209 00210 self.nunchukStickRaw = nunChuk['stick'] 00211 00212 # scale the joystick to roughly [-1, 1] 00213 if (self._nunchukJoystickZero is None): 00214 calibration = [127, 127] 00215 else: 00216 calibration = self._nunchukJoystickZero 00217 00218 [joyx, joyy] = self.nunchukStickRaw 00219 joyx = -(joyx-calibration[0])/100. 00220 joyy = (joyy-calibration[1])/100. 00221 # create a deadzone in the middle 00222 if abs(joyx) < .05: 00223 joyx = 0 00224 if abs(joyy) < .05: 00225 joyy = 0 00226 self.nunchukStick = [joyx,joyy] 00227 00228 nunButtons = nunChuk['buttons'] 00229 self.nunchukButtons[BTN_C] = (nunButtons & BTN_C) > 0 00230 self.nunchukButtons[BTN_Z] = (nunButtons & BTN_Z) > 0 00231 continue 00232 elif msgType == WII_MSG_TYPE_CLASSIC: 00233 clasSic = msgComp[1]; 00234 if clasSic is not None: 00235 self.classicPresent = True 00236 self.classicStickLeft = clasSic['l_stick'] 00237 self.classicStickRight = clasSic['r_stick'] 00238 clasButtons = clasSic['buttons'] 00239 self.classicButtons[CLASSIC_BTN_A] = (clasButtons & CLASSIC_BTN_A) > 0 00240 self.classicButtons[CLASSIC_BTN_B] = (clasButtons & CLASSIC_BTN_B) > 0 00241 self.classicButtons[CLASSIC_BTN_DOWN] = (clasButtons & CLASSIC_BTN_DOWN) > 0 00242 self.classicButtons[CLASSIC_BTN_HOME] = (clasButtons & CLASSIC_BTN_HOME) > 0 00243 self.classicButtons[CLASSIC_BTN_L] = (clasButtons & CLASSIC_BTN_L) > 0 00244 self.classicButtons[CLASSIC_BTN_LEFT] = (clasButtons & CLASSIC_BTN_LEFT) > 0 00245 self.classicButtons[CLASSIC_BTN_MINUS] = (clasButtons & CLASSIC_BTN_MINUS) > 0 00246 self.classicButtons[CLASSIC_BTN_PLUS] = (clasButtons & CLASSIC_BTN_PLUS) > 0 00247 self.classicButtons[CLASSIC_BTN_R] = (clasButtons & CLASSIC_BTN_R) > 0 00248 self.classicButtons[CLASSIC_BTN_RIGHT] = (clasButtons & CLASSIC_BTN_RIGHT) > 0 00249 self.classicButtons[CLASSIC_BTN_UP] = (clasButtons & CLASSIC_BTN_UP) > 0 00250 self.classicButtons[CLASSIC_BTN_X] = (clasButtons & CLASSIC_BTN_X) > 0 00251 self.classicButtons[CLASSIC_BTN_Y] = (clasButtons & CLASSIC_BTN_Y) > 0 00252 self.classicButtons[CLASSIC_BTN_ZL] = (clasButtons & CLASSIC_BTN_ZL) > 0 00253 self.classicButtons[CLASSIC_BTN_ZR] = (clasButtons & CLASSIC_BTN_ZR) > 0 00254 continue 00255 00256 00257 #---------------------------------------- 00258 # setAccelerometerCalibration 00259 #---------- 00260 00261 @classmethod 00262 def setAccelerometerCalibration(cls, zeroReading, oneReading): 00263 """Set the current accelerometer zeroing calibration.""" 00264 cls._accCalibrationZero = WIIReading(zeroReading) 00265 cls._accCalibrationOne = WIIReading(oneReading) 00266 00267 #---------------------------------------- 00268 # getAccelerometerCalibration 00269 #---------- 00270 00271 @classmethod 00272 def getAccelerometerCalibration(cls): 00273 """Return current accelerometer zeroing offset as two lists of x/y/z: the 00274 zero-reading, and the one-reading.""" 00275 return (cls._accCalibrationZero.tuple(), cls._accCalibrationOne.tuple()) 00276 00277 #---------------------------------------- 00278 # setGyroCalibration 00279 #---------- 00280 00281 @classmethod 00282 def setGyroCalibration(cls, zeroReading): 00283 """Set the x/y/z zeroing offsets for the gyro. Argument is a list""" 00284 00285 cls._gyroZeroReading = GyroReading(zeroReading) 00286 00287 #---------------------------------------- 00288 # getGyroCalibration 00289 #---------- 00290 00291 @classmethod 00292 def getGyroCalibration(cls): 00293 """Return current gyro zeroing offset as a list of x/y/z. """ 00294 return cls._gyroZeroReading.tuple() 00295 00296 #---------------------------------------- 00297 # setNunchukAccelerometerCalibration 00298 #---------- 00299 00300 @classmethod 00301 def setNunchukAccelerometerCalibration(cls, zeroReading, oneReading): 00302 """Set the current nunchuk accelerometer zeroing calibration.""" 00303 cls._nunchukZeroReading = WIIReading(zeroReading) 00304 cls._nunchukOneReading = WIIReading(oneReading) 00305 00306 #---------------------------------------- 00307 # setNunchukJoystickCalibration 00308 #---------- 00309 00310 @classmethod 00311 def setNunchukJoystickCalibration(cls, readings): 00312 """Set the origin for the nunchuk joystick""" 00313 cls._nunchukJoystickZero = readings 00314 00315 #---------------------------------------- 00316 # getNunchukAccelerometerCalibration 00317 #---------- 00318 00319 @classmethod 00320 def getNunchukAccelerometerCalibration(cls): 00321 """Return current nunchuk accelerometer zeroing offset as two lists of x/y/z: the 00322 zero-reading, and the one-reading.""" 00323 return (cls._nunchukZeroReading.tuple(), cls._nunchukOneReading.tuple()) 00324 00325 00326 #---------------------------------------- 00327 # __str___ 00328 #---------- 00329 00330 def __str__(self): 00331 00332 # Timestamp: 00333 res = 'Time: ' + self.ascTime + '\n' 00334 00335 # Buttons that are pressed: 00336 butRes = '' 00337 00338 if self.buttons is not None: 00339 if self.buttons[BTN_1]: 00340 butRes += ', 1' 00341 if self.buttons[BTN_2]: 00342 butRes += ', 2' 00343 if self.buttons[BTN_PLUS]: 00344 butRes += ', Plus' 00345 if self.buttons[BTN_MINUS]: 00346 butRes += ', Minus' 00347 if self.buttons[BTN_A]: 00348 butRes += ', A' 00349 if self.buttons[BTN_B]: 00350 butRes += ', B' 00351 if self.buttons[BTN_UP]: 00352 butRes += ', 4Way-up' 00353 if self.buttons[BTN_DOWN]: 00354 butRes += ', 4Way-down' 00355 if self.buttons[BTN_LEFT]: 00356 butRes += ', 4Way-left' 00357 if self.buttons[BTN_RIGHT]: 00358 butRes += ', 4Way-right' 00359 if self.buttons[BTN_HOME]: 00360 butRes += ', Home' 00361 00362 00363 # If none of the buttons is down, indicate that: 00364 if not butRes: 00365 res += 'Buttons: none.\n' 00366 else: 00367 res += 'Buttons: ' + butRes.lstrip(', ') + '\n' 00368 00369 00370 # Accelerator: 00371 if self.acc is not None: 00372 res += 'Accelerator: (' + \ 00373 `self.acc[X]` + ',' + \ 00374 `self.acc[Y]` + ',' + \ 00375 `self.acc[Z]` + ')\n' 00376 00377 # Gyro (angular rate): 00378 00379 if self.angleRate is not None: 00380 res += 'Gyro (angular rate): (' + \ 00381 `self.angleRate[X]` + ',' + \ 00382 `self.angleRate[Y]` + ',' + \ 00383 `self.angleRate[Z]` + ')\n' 00384 00385 # Rumble status: 00386 00387 if self.rumble: 00388 res += 'Rumble: On.\n' 00389 else: 00390 res += 'Rumble: Off.\n' 00391 00392 # IR Sources: 00393 00394 irRes = '' 00395 00396 if self.IRSources is not None: 00397 if self.IRSources[IR1] is not None: 00398 irRes += 'IR source 1' 00399 00400 if self.IRSources[IR2] is not None: 00401 irRes += 'IR source 2' 00402 00403 if self.IRSources[IR3] is not None: 00404 irRes += 'IR source 3' 00405 00406 if self.IRSources[IR4] is not None: 00407 irRes += 'IR source 4' 00408 00409 if not irRes: 00410 res += irRes.lstrip(', ') + '\n' 00411 else: 00412 res += 'No IR sources detected.\n' 00413 00414 00415 return res 00416 00417 #---------------------------------------- 00418 # __repr___ 00419 #---------- 00420 00421 def __repr__(self): 00422 return self.__str__() 00423 00424 00425 #---------------------------------------- 00426 # Class WIIReading 00427 #----------------- 00428 00429 class WIIReading(object): 00430 """Instances hold one 3-D reading. 00431 00432 Methods: 00433 [X], [Y], [Z] to obtain respective axis paramters. 00434 tuple() to obtain x/y/z as a NumPy array. 00435 +,-,/ to add or subtract readings from each other 00436 as one vector operation (pairwise for each dimension). 00437 """ 00438 00439 # Private instance vars: 00440 # o _measurement = np.array(3, dtype=numpy.float64) 00441 # o time 00442 00443 00444 def __init__(self, xyz, theTime=None): 00445 """Create a (possibly) time stamped WII Reading. 00446 00447 Parameter xyz is an array of x,y,z coordinates of the 00448 reading. WIIReading instances can be added, subtracted, and 00449 divided into each other. The operations are pairwise over 00450 x, y, and z. A numpy array of x,y,z is available by 00451 calling tuple(). The time stamp is available via time(). 00452 00453 """ 00454 self.time = theTime 00455 self._measurement = np.array([xyz[X], xyz[Y], xyz[Z]],dtype=np.float64) 00456 00457 def __getitem__(self, key): 00458 if key not in (X,Y,Z): 00459 raise AttributeError("Attempt to index into a 3-D measurement array with index " + `key` + ".") 00460 return self._measurement[key] 00461 00462 def __str__(self): 00463 return '[x=' + repr(self._measurement[X]) + \ 00464 ', y=' + repr(self._measurement[Y]) + \ 00465 ' z=' + repr(self._measurement[Z]) + \ 00466 ']' 00467 00468 def __repr__(self): 00469 return '[' + str(self._measurement[X]) + ', ' + str(self._measurement[Y]) + ', ' + str(self._measurement[Z]) + ']' 00470 00471 def tuple(self): 00472 return self._measurement 00473 00474 def __add__(self, other): 00475 """Adding two readings returns a numpy tuple with readings added pairwise.""" 00476 00477 return self._measurement + other._measurement 00478 00479 def __sub__(self, other): 00480 """Subtracting two readings returns a numpy tuple with components subtracted pairwise.""" 00481 00482 return self._measurement - other._measurement 00483 00484 def __div__(self, other): 00485 """Dividing two readings returns a numpy tuple with components divided pairwise.""" 00486 00487 return self._measurement / other._measurement 00488 00489 def scale(self, scaleFactor): 00490 """Return a numpy tuple that with X, Y, Z scaled by the given factor.""" 00491 00492 return self._measurement * scaleFactor 00493 00494 #---------------------------------------- 00495 # Class GyroReading 00496 #------------------ 00497 00498 class GyroReading(): 00499 """Instances hold one gyroscope reading. 00500 00501 Methods: 00502 [PHI], [THETA], [PSI] to obtain respective axis paramters. 00503 tuple() to obtain phi/theta/psi as a NumPy array. 00504 +,-,/ to add or subtract readings from each other 00505 as one vector operation (pairwise for each dimension). 00506 """ 00507 # Local instance vars: 00508 # o _measurement = np.array(3, dtype=numpy.float64) 00509 # o time 00510 00511 00512 def __init__(self, phiThetaPsi, theTime=None): 00513 """Create a (possibly) time stamped WII Reading. 00514 00515 Parameter phiThetaPsi is an array of phi,theta,psi coordinates of the 00516 gyro reading. GyroReading instances can be added, subtracted, and 00517 divided into each other. The operations are pairwise over 00518 phi, theta, and psi. A numpy array of phi,theta,psi is available by 00519 calling tuple(). The time stamp is available via time(). 00520 """ 00521 00522 self.time = theTime 00523 self._measurement = np.array([phiThetaPsi[PHI], phiThetaPsi[THETA], phiThetaPsi[PSI]],dtype=np.float64) 00524 00525 00526 def __getitem__(self, key): 00527 if key not in (PHI,THETA,PSI): 00528 raise AttributeError("Attempt to index into a 3-D measurement array with index " + `key` + ".") 00529 return self._measurement[key] 00530 00531 def __str__(self): 00532 return '[PHI (roll)=' + repr(self._measurement[PHI]) + \ 00533 ', THETA (pitch)=' + repr(self._measurement[THETA]) + \ 00534 ', PSI (yaw)=' + repr(self._measurement[PSI]) + \ 00535 ']' 00536 00537 def __repr__(self): 00538 '[' + str(self._measurement[PHI]) + ', ' + str(self._measurement[THETA]) + ', ' + str(self._measurement[PSI]) + ']' 00539 00540 def tuple(self): 00541 return self._measurement 00542 00543 def __add__(self, other): 00544 """Adding two gyro readings returns a new reading with components added pairwise.""" 00545 return self._measurement + other._measurement 00546 00547 def __sub__(self, other): 00548 """Subtracting two gyro readings returns a new reading 00549 with components subtracted pairwise. 00550 00551 """ 00552 return self._measurement - other._measurement 00553 00554 def __div__(self, other): 00555 """Dividing two readings returns a numpy tuple with components divided pairwise.""" 00556 00557 return self._measurement / other._measurement 00558 00559 def scale(self, scaleFactor): 00560 """Return a numpy tuple that with X, Y, Z scaled by the given factor.""" 00561 00562 return self._measurement * scaleFactor 00563 00564 #;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 00565 # 00566 # Utility Functions 00567 # 00568 #;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;