wiistate.py
Go to the documentation of this file.
1 ################################################################################
2 #
3 # File: wiistate.py
4 # RCS: $Header: $
5 # Description: Object representation of Wiimote's state
6 # Author: Andreas Paepcke
7 # Created: Thu Aug 13 11:01:34 2009 (Andreas Paepcke) paepcke@anw.willowgarage.com
8 # Modified: Thu Jan 13 13:50:15 2011 (Andreas Paepcke) paepcke@bhb.willowgarage.com
9 # Language: Python
10 # Package: N/A
11 # Status: Experimental (Do Not Distribute)
12 #
13 #
14 #
15 ################################################################################
16 #
17 # Revisions:
18 #
19 # Thu Mar 18 10:56:09 2010 (David Lu) davidlu@wustl.edu
20 # Added nunchuk state variables
21 # Fri Oct 29 08:58:21 2010 (Miguel Angel Julian Aguilar, QBO Project) miguel.angel@thecorpora.com
22 # Added classic controller state variables
23 # Mon Nov 08 11:43:23 2010 (David Lu) davidlu@wustl.edu
24 # Added calibration for nunchuk
25 ################################################################################
26 
27 from wiimoteConstants import *
28 from wiiutils import *
29 import numpy as np
30 
31 #----------------------------------------
32 # Class WIIState
33 #---------------
34 
35 class WIIState(object):
36  """Holds the state of a WIIRemote-plus.
37 
38  The state is passed in and is as communicated
39  by one message from the WII+ device. We unpack
40  the information and place it into individual
41  dictionaries for callers to grab.
42 
43  Public instance variables:
44  o time Time in fractional seconds since beginning of Epoch of when
45  state was measured (Float).
46  o ascTime Time when state was measured (Human-readable)
47  o rumble True/False if wiimote vibration is on/off
48  o angleRate A GyroReading instance containing gyro (a.k.a. angular rate) measurement
49  o acc A WIIReading instance containing accelerometer measurement corrected by
50  the calibration information that is stored in the Wiimote
51  o accRaw A WIIReading instance containing accelerometer measurement uncorrected
52  o buttons A dictionary for which buttons are being held down. That could be
53  multiple buttons. Keys are:
54  BTN_1, BTN_2, BTN_PLUS, BTN_MINUS, BTN_A, BTN_B,
55  BTN_UP, BTN_DOWN, BTN_LEFT, BTN_RIGHT, BTN_HOME
56  Values are 1/0
57  o IRSources Dictionary with on/off values for which IR lights are
58  being sensed. Keys are:
59  IR1, IR2, IR3, IR4
60  Values are 1/0
61  o motionPlusPresent True if a gyro Motion+ is plugged into the Wiimote. Else False
62 
63  o nunchukPresent True if nunchuk is plugged in. Else False
64  o nunchukAccRaw A WIIReading instance with acceleromoter measurement from the nunchuk (raw values)
65  o nunchukAcc The same, but zeroed using factory calibration
66  o nunchukStickRaw A tuple with the two axes of the joystick on the nunchuk, raw readings
67  o nunchukStick A tuple with the two axes of the joystick on the nunchuk, zeroed to be [-1, 1]
68  o nunchukButtons A dictionary for which nunchuk buttons are down. Keys are BTN_C and BTN_Z
69 
70  Public methods:
71  o setAccelerometerCalibration Bias setting for accelerometer. This triplet is used to
72  turn raw accelerometer values into calibrated values.
73  o setGyroCalibration Bias setting for gyro. This triplet is used to
74  turn raw gyro values into calibrated values.
75  """
76 
77  _accCalibrationZero = None
78  _gyroZeroReading = None
79  _nunchukZeroReading = None
80  _nunchukJoystickZero = None
81 
82  #----------------------------------------
83  # __init__
84  #----------
85 
86  def __init__(self, state, theTime, theRumble, buttonStatus):
87  """Unpack the given state, normalizing if normalizers are passed in."""
88 
89  self.time = theTime
90  self.ascTime = `theTime`
91  self.rumble = theRumble
92  self.IRSources = [None, None, None, None]
93  self.battery = None
94  self.acc = None
95  self.accRaw = None
96  self.angleRate = None
97  self.angleRate = None
98  self.angleRageRaw = None
99  self.motionPlusPresent = False
100  self.buttons = {BTN_1: False, BTN_2: False, BTN_PLUS: False,
101  BTN_MINUS: False, BTN_A: False, BTN_B: False,
102  BTN_UP: False, BTN_DOWN: False, BTN_LEFT: False,
103  BTN_RIGHT: False, BTN_HOME: False}
104  self.nunchukPresent = False
105  self.nunchukAccRaw = None
106  self.nunchukAcc = None
107  self.nunchukStick = None
108  self.nunchukStickRaw = None
109  self.nunchukButtons = {BTN_C: False, BTN_Z: False}
110 
111  self.classicPresent = False
112  self.classicStickLeft = None
113  self.classicStickRight = None
114  self.classicButtons = {CLASSIC_BTN_A: False, CLASSIC_BTN_B: False,
115  CLASSIC_BTN_L: False, CLASSIC_BTN_R: False,
116  CLASSIC_BTN_X: False, CLASSIC_BTN_Y: False,
117  CLASSIC_BTN_ZL: False, CLASSIC_BTN_ZR: False,
118  CLASSIC_BTN_PLUS: False, CLASSIC_BTN_MINUS: False,
119  CLASSIC_BTN_UP: False, CLASSIC_BTN_DOWN: False,
120  CLASSIC_BTN_LEFT: False, CLASSIC_BTN_RIGHT: False,
121  CLASSIC_BTN_HOME: False}
122 
123  # Handle buttons on the WII
124  # A zero means no button is down.
125 
126  if buttonStatus == 0:
127  for key in self.buttons.keys():
128  self.buttons[key] = False
129  continue
130 
131  self.buttons[BTN_1] = (buttonStatus & BTN_1) > 0
132  self.buttons[BTN_2] = (buttonStatus & BTN_2) > 0
133  self.buttons[BTN_PLUS] = (buttonStatus & BTN_PLUS) > 0
134  self.buttons[BTN_MINUS] = (buttonStatus & BTN_MINUS) > 0
135  self.buttons[BTN_A] = (buttonStatus & BTN_A) > 0
136  self.buttons[BTN_B] = (buttonStatus & BTN_B) > 0
137  self.buttons[BTN_UP] = (buttonStatus & BTN_UP) > 0
138  self.buttons[BTN_DOWN] = (buttonStatus & BTN_DOWN) > 0
139  self.buttons[BTN_LEFT] = (buttonStatus & BTN_LEFT) > 0
140  self.buttons[BTN_RIGHT] = (buttonStatus & BTN_RIGHT) > 0
141  self.buttons[BTN_HOME] = (buttonStatus & BTN_HOME) > 0
142 
143  for msgComp in state:
144  # msgComp has: (1,2) for Status:Button one pushed, or
145  # (3, [None,None,None,None]) for LEDs
146  # (7, {'angle_rage': (123,456,789)})
147  msgType = msgComp[0]
148 
149  if msgType == WII_MSG_TYPE_ACC:
150  # Second list member is accelerator triplet of numbers:
151  accStatus = msgComp[1]
152  self.accRaw = WIIReading(accStatus, self.time)
153 
154  # If this class knows about accelerometer calibration
155  # data, correct the raw reading:
156  if self._accCalibrationZero is not None and self._accCalibrationOne is not None and np.linalg.norm(self._accCalibrationOne - self._accCalibrationZero) > 1E-5:
157  self.acc = WIIReading((self.accRaw - self._accCalibrationZero) / (self._accCalibrationOne - self._accCalibrationZero), self.time)
158  else:
159  self.acc = WIIReading(self.accRaw, self.time)
160 
161  continue
162 
163  elif msgType == WII_MSG_TYPE_IR:
164  # Second list member is a list of 4 dictionaries,
165  # one for each of the Wii IR sources:
166 
167  IRStatus = msgComp[1]
168  # IRStatus is an array of four Dicts. Ex: [{'pos': (317, 445)}, None, None, None]
169  self.IRSources[0] = IRStatus[0]
170  self.IRSources[1] = IRStatus[1]
171  self.IRSources[2] = IRStatus[2]
172  self.IRSources[3] = IRStatus[3]
173 
174  continue
175 
176  elif msgType == WII_MSG_TYPE_MOTIONPLUS:
177  # Second list member is a dictionary with the single
178  # key 'angle_rate', which yields as its value a gyro
179  # readings triplet of numbers:
180 
181  gyroDict = msgComp[1]
182 
183  if gyroDict is not None:
184  # If this class knows about a zero-movement reading for the
185  # gyro, subtract that reading from the raw measurement:
186 
187  self.angleRateRaw = GyroReading(gyroDict['angle_rate'], self.time)
188  if self._gyroZeroReading is not None:
189  self.angleRate = GyroReading(self.angleRateRaw - self._gyroZeroReading, self.time)
190  else:
191  self.angleRate = self.angleRateRaw
192 
193  self.motionPlusPresent = True
194 
195  continue
196  elif msgType == WII_MSG_TYPE_NUNCHUK:
197  nunChuk = msgComp[1];
198  if nunChuk is not None:
199  self.nunchukPresent = True
200  self.nunchukAccRaw = WIIReading(nunChuk['acc'], self.time)
201 
202  # If this class knows about accelerometer calibration
203  # data, correct the raw reading:
204  if self._nunchukZeroReading is not None:
206  else:
207  self.nunchukAcc = WIIReading(self.nunchukAccRaw, self.time)
208 
209 
210  self.nunchukStickRaw = nunChuk['stick']
211 
212  # scale the joystick to roughly [-1, 1]
213  if (self._nunchukJoystickZero is None):
214  calibration = [127, 127]
215  else:
216  calibration = self._nunchukJoystickZero
217 
218  [joyx, joyy] = self.nunchukStickRaw
219  joyx = -(joyx-calibration[0])/100.
220  joyy = (joyy-calibration[1])/100.
221  # create a deadzone in the middle
222  if abs(joyx) < .05:
223  joyx = 0
224  if abs(joyy) < .05:
225  joyy = 0
226  self.nunchukStick = [joyx,joyy]
227 
228  nunButtons = nunChuk['buttons']
229  self.nunchukButtons[BTN_C] = (nunButtons & BTN_C) > 0
230  self.nunchukButtons[BTN_Z] = (nunButtons & BTN_Z) > 0
231  continue
232  elif msgType == WII_MSG_TYPE_CLASSIC:
233  clasSic = msgComp[1];
234  if clasSic is not None:
235  self.classicPresent = True
236  self.classicStickLeft = clasSic['l_stick']
237  self.classicStickRight = clasSic['r_stick']
238  clasButtons = clasSic['buttons']
239  self.classicButtons[CLASSIC_BTN_A] = (clasButtons & CLASSIC_BTN_A) > 0
240  self.classicButtons[CLASSIC_BTN_B] = (clasButtons & CLASSIC_BTN_B) > 0
241  self.classicButtons[CLASSIC_BTN_DOWN] = (clasButtons & CLASSIC_BTN_DOWN) > 0
242  self.classicButtons[CLASSIC_BTN_HOME] = (clasButtons & CLASSIC_BTN_HOME) > 0
243  self.classicButtons[CLASSIC_BTN_L] = (clasButtons & CLASSIC_BTN_L) > 0
244  self.classicButtons[CLASSIC_BTN_LEFT] = (clasButtons & CLASSIC_BTN_LEFT) > 0
245  self.classicButtons[CLASSIC_BTN_MINUS] = (clasButtons & CLASSIC_BTN_MINUS) > 0
246  self.classicButtons[CLASSIC_BTN_PLUS] = (clasButtons & CLASSIC_BTN_PLUS) > 0
247  self.classicButtons[CLASSIC_BTN_R] = (clasButtons & CLASSIC_BTN_R) > 0
248  self.classicButtons[CLASSIC_BTN_RIGHT] = (clasButtons & CLASSIC_BTN_RIGHT) > 0
249  self.classicButtons[CLASSIC_BTN_UP] = (clasButtons & CLASSIC_BTN_UP) > 0
250  self.classicButtons[CLASSIC_BTN_X] = (clasButtons & CLASSIC_BTN_X) > 0
251  self.classicButtons[CLASSIC_BTN_Y] = (clasButtons & CLASSIC_BTN_Y) > 0
252  self.classicButtons[CLASSIC_BTN_ZL] = (clasButtons & CLASSIC_BTN_ZL) > 0
253  self.classicButtons[CLASSIC_BTN_ZR] = (clasButtons & CLASSIC_BTN_ZR) > 0
254  continue
255 
256 
257  #----------------------------------------
258  # setAccelerometerCalibration
259  #----------
260 
261  @classmethod
262  def setAccelerometerCalibration(cls, zeroReading, oneReading):
263  """Set the current accelerometer zeroing calibration."""
264  cls._accCalibrationZero = WIIReading(zeroReading)
265  cls._accCalibrationOne = WIIReading(oneReading)
266 
267  #----------------------------------------
268  # getAccelerometerCalibration
269  #----------
270 
271  @classmethod
273  """Return current accelerometer zeroing offset as two lists of x/y/z: the
274  zero-reading, and the one-reading."""
275  return (cls._accCalibrationZero.tuple(), cls._accCalibrationOne.tuple())
276 
277  #----------------------------------------
278  # setGyroCalibration
279  #----------
280 
281  @classmethod
282  def setGyroCalibration(cls, zeroReading):
283  """Set the x/y/z zeroing offsets for the gyro. Argument is a list"""
284 
285  cls._gyroZeroReading = GyroReading(zeroReading)
286 
287  #----------------------------------------
288  # getGyroCalibration
289  #----------
290 
291  @classmethod
293  """Return current gyro zeroing offset as a list of x/y/z. """
294  return cls._gyroZeroReading.tuple()
295 
296  #----------------------------------------
297  # setNunchukAccelerometerCalibration
298  #----------
299 
300  @classmethod
301  def setNunchukAccelerometerCalibration(cls, zeroReading, oneReading):
302  """Set the current nunchuk accelerometer zeroing calibration."""
303  cls._nunchukZeroReading = WIIReading(zeroReading)
304  cls._nunchukOneReading = WIIReading(oneReading)
305 
306  #----------------------------------------
307  # setNunchukJoystickCalibration
308  #----------
309 
310  @classmethod
311  def setNunchukJoystickCalibration(cls, readings):
312  """Set the origin for the nunchuk joystick"""
313  cls._nunchukJoystickZero = readings
314 
315  #----------------------------------------
316  # getNunchukAccelerometerCalibration
317  #----------
318 
319  @classmethod
321  """Return current nunchuk accelerometer zeroing offset as two lists of x/y/z: the
322  zero-reading, and the one-reading."""
323  return (cls._nunchukZeroReading.tuple(), cls._nunchukOneReading.tuple())
324 
325 
326  #----------------------------------------
327  # __str___
328  #----------
329 
330  def __str__(self):
331 
332  # Timestamp:
333  res = 'Time: ' + self.ascTime + '\n'
334 
335  # Buttons that are pressed:
336  butRes = ''
337 
338  if self.buttons is not None:
339  if self.buttons[BTN_1]:
340  butRes += ', 1'
341  if self.buttons[BTN_2]:
342  butRes += ', 2'
343  if self.buttons[BTN_PLUS]:
344  butRes += ', Plus'
345  if self.buttons[BTN_MINUS]:
346  butRes += ', Minus'
347  if self.buttons[BTN_A]:
348  butRes += ', A'
349  if self.buttons[BTN_B]:
350  butRes += ', B'
351  if self.buttons[BTN_UP]:
352  butRes += ', 4Way-up'
353  if self.buttons[BTN_DOWN]:
354  butRes += ', 4Way-down'
355  if self.buttons[BTN_LEFT]:
356  butRes += ', 4Way-left'
357  if self.buttons[BTN_RIGHT]:
358  butRes += ', 4Way-right'
359  if self.buttons[BTN_HOME]:
360  butRes += ', Home'
361 
362 
363  # If none of the buttons is down, indicate that:
364  if not butRes:
365  res += 'Buttons: none.\n'
366  else:
367  res += 'Buttons: ' + butRes.lstrip(', ') + '\n'
368 
369 
370  # Accelerator:
371  if self.acc is not None:
372  res += 'Accelerator: (' + \
373  `self.acc[X]` + ',' + \
374  `self.acc[Y]` + ',' + \
375  `self.acc[Z]` + ')\n'
376 
377  # Gyro (angular rate):
378 
379  if self.angleRate is not None:
380  res += 'Gyro (angular rate): (' + \
381  `self.angleRate[X]` + ',' + \
382  `self.angleRate[Y]` + ',' + \
383  `self.angleRate[Z]` + ')\n'
384 
385  # Rumble status:
386 
387  if self.rumble:
388  res += 'Rumble: On.\n'
389  else:
390  res += 'Rumble: Off.\n'
391 
392  # IR Sources:
393 
394  irRes = ''
395 
396  if self.IRSources is not None:
397  if self.IRSources[IR1] is not None:
398  irRes += 'IR source 1'
399 
400  if self.IRSources[IR2] is not None:
401  irRes += 'IR source 2'
402 
403  if self.IRSources[IR3] is not None:
404  irRes += 'IR source 3'
405 
406  if self.IRSources[IR4] is not None:
407  irRes += 'IR source 4'
408 
409  if not irRes:
410  res += irRes.lstrip(', ') + '\n'
411  else:
412  res += 'No IR sources detected.\n'
413 
414 
415  return res
416 
417  #----------------------------------------
418  # __repr___
419  #----------
420 
421  def __repr__(self):
422  return self.__str__()
423 
424 
425 #----------------------------------------
426 # Class WIIReading
427 #-----------------
428 
429 class WIIReading(object):
430  """Instances hold one 3-D reading.
431 
432  Methods:
433  [X], [Y], [Z] to obtain respective axis paramters.
434  tuple() to obtain x/y/z as a NumPy array.
435  +,-,/ to add or subtract readings from each other
436  as one vector operation (pairwise for each dimension).
437  """
438 
439  # Private instance vars:
440  # o _measurement = np.array(3, dtype=numpy.float64)
441  # o time
442 
443 
444  def __init__(self, xyz, theTime=None):
445  """Create a (possibly) time stamped WII Reading.
446 
447  Parameter xyz is an array of x,y,z coordinates of the
448  reading. WIIReading instances can be added, subtracted, and
449  divided into each other. The operations are pairwise over
450  x, y, and z. A numpy array of x,y,z is available by
451  calling tuple(). The time stamp is available via time().
452 
453  """
454  self.time = theTime
455  self._measurement = np.array([xyz[X], xyz[Y], xyz[Z]],dtype=np.float64)
456 
457  def __getitem__(self, key):
458  if key not in (X,Y,Z):
459  raise AttributeError("Attempt to index into a 3-D measurement array with index " + `key` + ".")
460  return self._measurement[key]
461 
462  def __str__(self):
463  return '[x=' + repr(self._measurement[X]) + \
464  ', y=' + repr(self._measurement[Y]) + \
465  ' z=' + repr(self._measurement[Z]) + \
466  ']'
467 
468  def __repr__(self):
469  return '[' + str(self._measurement[X]) + ', ' + str(self._measurement[Y]) + ', ' + str(self._measurement[Z]) + ']'
470 
471  def tuple(self):
472  return self._measurement
473 
474  def __add__(self, other):
475  """Adding two readings returns a numpy tuple with readings added pairwise."""
476 
477  return self._measurement + other._measurement
478 
479  def __sub__(self, other):
480  """Subtracting two readings returns a numpy tuple with components subtracted pairwise."""
481 
482  return self._measurement - other._measurement
483 
484  def __div__(self, other):
485  """Dividing two readings returns a numpy tuple with components divided pairwise."""
486 
487  return self._measurement / other._measurement
488 
489  def scale(self, scaleFactor):
490  """Return a numpy tuple that with X, Y, Z scaled by the given factor."""
491 
492  return self._measurement * scaleFactor
493 
494 #----------------------------------------
495 # Class GyroReading
496 #------------------
497 
498 class GyroReading():
499  """Instances hold one gyroscope reading.
500 
501  Methods:
502  [PHI], [THETA], [PSI] to obtain respective axis paramters.
503  tuple() to obtain phi/theta/psi as a NumPy array.
504  +,-,/ to add or subtract readings from each other
505  as one vector operation (pairwise for each dimension).
506  """
507  # Local instance vars:
508  # o _measurement = np.array(3, dtype=numpy.float64)
509  # o time
510 
511 
512  def __init__(self, phiThetaPsi, theTime=None):
513  """Create a (possibly) time stamped WII Reading.
514 
515  Parameter phiThetaPsi is an array of phi,theta,psi coordinates of the
516  gyro reading. GyroReading instances can be added, subtracted, and
517  divided into each other. The operations are pairwise over
518  phi, theta, and psi. A numpy array of phi,theta,psi is available by
519  calling tuple(). The time stamp is available via time().
520  """
521 
522  self.time = theTime
523  self._measurement = np.array([phiThetaPsi[PHI], phiThetaPsi[THETA], phiThetaPsi[PSI]],dtype=np.float64)
524 
525 
526  def __getitem__(self, key):
527  if key not in (PHI,THETA,PSI):
528  raise AttributeError("Attempt to index into a 3-D measurement array with index " + `key` + ".")
529  return self._measurement[key]
530 
531  def __str__(self):
532  return '[PHI (roll)=' + repr(self._measurement[PHI]) + \
533  ', THETA (pitch)=' + repr(self._measurement[THETA]) + \
534  ', PSI (yaw)=' + repr(self._measurement[PSI]) + \
535  ']'
536 
537  def __repr__(self):
538  '[' + str(self._measurement[PHI]) + ', ' + str(self._measurement[THETA]) + ', ' + str(self._measurement[PSI]) + ']'
539 
540  def tuple(self):
541  return self._measurement
542 
543  def __add__(self, other):
544  """Adding two gyro readings returns a new reading with components added pairwise."""
545  return self._measurement + other._measurement
546 
547  def __sub__(self, other):
548  """Subtracting two gyro readings returns a new reading
549  with components subtracted pairwise.
550 
551  """
552  return self._measurement - other._measurement
553 
554  def __div__(self, other):
555  """Dividing two readings returns a numpy tuple with components divided pairwise."""
556 
557  return self._measurement / other._measurement
558 
559  def scale(self, scaleFactor):
560  """Return a numpy tuple that with X, Y, Z scaled by the given factor."""
561 
562  return self._measurement * scaleFactor
563 
564 #;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
565 #
566 # Utility Functions
567 #
568 #;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
def scale(self, scaleFactor)
Definition: wiistate.py:559
def __add__(self, other)
Definition: wiistate.py:543
def setAccelerometerCalibration(cls, zeroReading, oneReading)
Definition: wiistate.py:262
def setNunchukAccelerometerCalibration(cls, zeroReading, oneReading)
Definition: wiistate.py:301
def __getitem__(self, key)
Definition: wiistate.py:526
def setNunchukJoystickCalibration(cls, readings)
Definition: wiistate.py:311
def getNunchukAccelerometerCalibration(cls)
Definition: wiistate.py:320
def __init__(self, xyz, theTime=None)
Definition: wiistate.py:444
def __div__(self, other)
Definition: wiistate.py:554
def setGyroCalibration(cls, zeroReading)
Definition: wiistate.py:282
def __init__(self, state, theTime, theRumble, buttonStatus)
Definition: wiistate.py:86
def scale(self, scaleFactor)
Definition: wiistate.py:489
def __sub__(self, other)
Definition: wiistate.py:479
def __init__(self, phiThetaPsi, theTime=None)
Definition: wiistate.py:512
def __add__(self, other)
Definition: wiistate.py:474
def getAccelerometerCalibration(cls)
Definition: wiistate.py:272
def __getitem__(self, key)
Definition: wiistate.py:457
def __sub__(self, other)
Definition: wiistate.py:547
def __div__(self, other)
Definition: wiistate.py:484


wiimote
Author(s): Andreas Paepcke, Melonee Wise, Mark Horn
autogenerated on Mon Jun 10 2019 13:42:43