00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 from wiimoteConstants import *
00028 from wiiutils import *
00029 import numpy as np
00030
00031
00032
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
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
00124
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
00145
00146
00147 msgType = msgComp[0]
00148
00149 if msgType == WII_MSG_TYPE_ACC:
00150
00151 accStatus = msgComp[1]
00152 self.accRaw = WIIReading(accStatus, self.time)
00153
00154
00155
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
00165
00166
00167 IRStatus = msgComp[1]
00168
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
00178
00179
00180
00181 gyroDict = msgComp[1]
00182
00183 if gyroDict is not None:
00184
00185
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
00203
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
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
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
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
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
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
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
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
00308
00309
00310 @classmethod
00311 def setNunchukJoystickCalibration(cls, readings):
00312 """Set the origin for the nunchuk joystick"""
00313 cls._nunchukJoystickZero = readings
00314
00315
00316
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
00328
00329
00330 def __str__(self):
00331
00332
00333 res = 'Time: ' + self.ascTime + '\n'
00334
00335
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
00364 if not butRes:
00365 res += 'Buttons: none.\n'
00366 else:
00367 res += 'Buttons: ' + butRes.lstrip(', ') + '\n'
00368
00369
00370
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
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
00386
00387 if self.rumble:
00388 res += 'Rumble: On.\n'
00389 else:
00390 res += 'Rumble: Off.\n'
00391
00392
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
00419
00420
00421 def __repr__(self):
00422 return self.__str__()
00423
00424
00425
00426
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
00440
00441
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
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
00508
00509
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
00567
00568