WIIMote.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 ################################################################################
00003 #
00004 # File:         WIIMote.py
00005 # RCS:          $Header: $
00006 # Description:  Top Level Wii Remote Control
00007 # Author:       Andreas Paepcke
00008 # Created:      Thu Aug 13 09:00:27 2009
00009 # Modified:     Fri Jan 14 10:48:48 2011 (Andreas Paepcke) paepcke@bhb.willowgarage.com
00010 # Language:     Python
00011 # Package:      N/A
00012 # Status:       Experimental (Do Not Distribute)
00013 #
00014 # 
00015 ################################################################################
00016 #
00017 # Revisions:
00018 #
00019 # Fri Jan 14 10:48:11 2011 (Andreas Paepcke) paepcke@bhb.willowgarage.com
00020 #  Added warning to ignore error messages when neither Nunchuk nor WiimotePlus
00021 #  are present.
00022 # Thu Jan 13 17:29:06 2011 (Andreas Paepcke) paepcke@bhb.willowgarage.com
00023 #  Added shutdown exception guard in getRumble()
00024 # Thu Sep 10 10:27:38 2009 (Andreas Paepcke) paepcke@anw.willowgarage.com
00025 #  Added option to lock access to wiiMoteState instance variable.
00026 # Thu Mar 18 10:56:09 2010 (David Lu) davidlu@wustl.edu
00027 #  Enabled nunchuk reports
00028 # Fri Oct 29 08:58:21 2010 (Miguel Angel Julian Aguilar, QBO Project) miguel.angel@thecorpora.com
00029 #  Enabled classic controller reports
00030 # Mon Nov 08 11:44:39 2010 (David Lu) davidlu@wustl.edu
00031 #  Added nunchuk calibration
00032 ################################################################################
00033 
00034 # ROS-Related Imports
00035 
00036 # Python-Internal Imports
00037 
00038 import operator
00039 import time
00040 import sys
00041 import threading
00042 from math import *
00043 import tempfile
00044 import os
00045 
00046 # Third party modules:
00047 
00048 import cwiid
00049 import numpy as np
00050 # ROS modules:
00051 
00052 import rospy
00053 
00054 # Wiimote modules:
00055 
00056 from wiiutils import *
00057 from wiimoteExceptions import *
00058 from wiimoteConstants import *
00059 import wiistate
00060 
00061 #;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
00062 #
00063 #    Global Constants
00064 #
00065 #;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
00066 
00067 # Note: the Wiimote object in _wm provides a dictionary
00068 #       of some Wiimote state:
00069 #      _wm.state: {'led': 0, 'rpt_mode': 2, 'ext_type': 4, 'buttons': 0, 'rumble': 0, 'error': 0, 'battery': 85}
00070 #;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
00071 #
00072 #    Class WIIMote
00073 #
00074 #;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
00075 
00076 
00077 class WIIMote(object):
00078   """Main class for Wiimote device interaction.
00079   
00080   This class should be a singleton, or it should have
00081   only class members/methods.
00082   
00083   Public Data attributes:
00084       wiiMoteState   WIIState object that holds the latest sampled state
00085       sampleRate     Control Wiimote state samples to take per second
00086       meanAcc        Triplet with mean of accelerator at rest
00087       stdevAcc       Triplet with standard deviation of accelerator at rest
00088       meanGyro       Triplet with mean of gyro (angular rate) at rest
00089       stdevGyro      Triplet with standard deviation gyro (angular rate) at rest
00090       
00091   Public Methods:
00092       
00093   
00094   """
00095 
00096   # Public constants:
00097   
00098   BATTERY_MAX = cwiid.BATTERY_MAX  # 208 a.k.a. 0xD0
00099   
00100   # Public vars:
00101 
00102   
00103   # Private constants:
00104 
00105   
00106   # Private vars:
00107 
00108   _wm = None                 # WIIMote object
00109   _wiiCallbackStack = None   # Stack for directing Wii driver callbacks
00110 
00111   _startTime = None          # Used for state sampling
00112   _accList = None            # Collecting accelerator readings for zeroing and others
00113   _gyroList = None
00114   _readingsCnt = None        # For counting how many readings were taken
00115   _accTotal = None           # Summed up acc readings in one AccReading instance
00116   _gyroTotal = None          # Summed up gyro readings in one AccReading instance
00117   
00118 
00119   _accNormal = None          # Readings of accelerometer at rest
00120   _gyroNormal = None         # Readings of gyro at rest
00121 
00122   _nunchukJoyOrig = None     # Initial Reading of the nunchuk's joystick
00123   
00124   _LEDMasksOn = [LED1_ON, LED2_ON, LED3_ON, LED4_ON] # OR to turn on
00125   _LEDMasksOff = [0 | LED2_ON | LED3_ON | LED4_ON, # AND to turn off
00126                   0 | LED1_ON | LED3_ON | LED4_ON,
00127                   0 | LED1_ON | LED2_ON | LED4_ON,
00128                   0 | LED1_ON | LED2_ON | LED3_ON]
00129 
00130 
00131   #----------------------------------------
00132   # __init__
00133   #------------------
00134 
00135   def __init__(self, theSampleRate=0, wiiStateLock=None, gatherCalibrationStats=False):
00136     """Instantiate a Wiimote driver instance, which controls one physical Wiimote device.
00137     
00138     Parameters:
00139         theSampleRate: How often to update the instance's wiiMoteState variable:
00140             theSampleRate= -1: never
00141             theSampleRate=  0: as often as possible
00142             theSampleRate=  x: every x seconds   
00143     """
00144 
00145     self.lastZeroingTime = 0.
00146     
00147     self.gatherCalibrationStats = gatherCalibrationStats
00148     if (self.gatherCalibrationStats):
00149         self.calibrationSamples = []
00150         for i in range(NUM_ZEROING_READINGS):
00151             self.calibrationSamples.append(CalibrationMeasurements())
00152         
00153     # Create a threading.Lock instance.
00154     # The instance variable wiiMoteState is only updated after acquiring that
00155     # lock. This is true for both reading and writing. The same is
00156     # true for accesses to: meanAcc, stdevAcc, varAcc, stdevGyro, and varGyro
00157     # All such accesses happen w/in this class, b/c we have accessors for
00158     # these variables. The lock is used also in method zeroDevice() as 
00159     # well, to prevent other threads from retrieving bad values between
00160     # the time zeroDevice() begins and ends:
00161     
00162     self.wiiStateLock = threading.Lock()
00163     self.wiiMoteState = None        # Object holding a snapshot of the Wiimote state
00164     self.sampleRate = -1            # How often to update wiiMoteState
00165                                #    -1: Never
00166                                #     0: Everytime the underlying system offers state
00167                                #  else: (Possibly fractional) seconds between updates
00168   
00169     # Mean x/y/z of most recent accelerometer zeroing in Gs and metric:
00170     self.meanAcc = np.array([None, None, None],dtype=np.float64)   
00171     self.meanAccMetric = np.array([None, None, None],dtype=np.float64)
00172     # Stdev x/y/z of most recent accelerometer zeroing in Gs and metric:
00173     self.stdevAcc = np.array([None, None, None],dtype=np.float64)        
00174     self.stdevAccMetric = np.array([None, None, None],dtype=np.float64)
00175     # Variance x/y/z of most recent accelerometer zeroing                    
00176     self.varAcc = np.array([None, None, None],dtype=np.float64)
00177     
00178     # Mean x/y/z of most recent gyro zeroing in Gs and metric:
00179     self.meanGyro = np.array([None, None, None],dtype=np.float64)
00180     self.meanGyroMetric = np.array([None, None, None],dtype=np.float64)
00181     # Stdev x/y/z of most recent gyro zeroing in Gs and metric:
00182     self.stdevGyro = np.array([None, None, None],dtype=np.float64)
00183     self.stdevGyroMetric = np.array([None, None, None],dtype=np.float64)
00184     # Variance x/y/z of most recent gyro zeroing                                 
00185     self.varGyroMetric = np.array([None, None, None],dtype=np.float64)
00186                                  
00187     self.latestCalibrationSuccessful = False;
00188     
00189     promptUsr("Press buttons 1 and 2 together to pair (within 6 seconds).\n    (If no blinking lights, press power button for ~3 seconds.)")
00190 
00191     try:
00192       self._wm = cwiid.Wiimote()
00193     except RuntimeError:
00194       raise WiimoteNotFoundError("No Wiimote found to pair with.")
00195       exit()
00196 
00197     rospy.loginfo("Pairing successful.")
00198 
00199     try:
00200       self._wm.enable(cwiid.FLAG_MOTIONPLUS)
00201     except RuntimeError:
00202       raise WiimoteEnableError("Found Wiimote, but could not enable it.")
00203       exit
00204       
00205     self.sampleRate = theSampleRate
00206     self._startTime = getTimeStamp();
00207 
00208     self._wiiCallbackStack = _WiiCallbackStack(self._wm)
00209 
00210     # Enable reports from the WII:
00211     self._wm.rpt_mode = cwiid.RPT_ACC | cwiid.RPT_MOTIONPLUS | cwiid.RPT_BTN | cwiid.RPT_IR | cwiid.RPT_NUNCHUK | cwiid.RPT_CLASSIC
00212     
00213     # Set accelerometer calibration to factory defaults:
00214     (factoryZero, factoryOne) = self.getAccFactoryCalibrationSettings()
00215     self.setAccelerometerCalibration(factoryZero, factoryOne)
00216     
00217     # Initialize Gyro zeroing to do nothing:
00218     self.setGyroCalibration([0,0,0])
00219 
00220     # Set nunchuk calibration to factory defaults.
00221     # Unfortunately, if neither WiimotePlus nor nunchuk are attached
00222     # we get two error msgs to the screen from the lowest cwiid levels.
00223     # TODO: suppress those two error messages ('Wiimote read error' and
00224     #       'Read error (nunchuk cal)'
00225     rospy.loginfo("Checking presence of Nunchuk attachment ...ignore two possibly following error msgs.")
00226     try:
00227       (factoryZero, factoryOne) = self.getNunchukFactoryCalibrationSettings()
00228       self.setNunchukAccelerometerCalibration(factoryZero, factoryOne)
00229     except:
00230       pass
00231 
00232     time.sleep(0.2)
00233     self._wiiCallbackStack.push(self._steadyStateCallback)
00234 
00235     rospy.loginfo("Wiimote activated.")
00236 
00237 
00238   #----------------------------------------
00239   # steadyStateCallback
00240   #------------------
00241 
00242   def _steadyStateCallback(self, state, theTime):
00243     #print state
00244     now = getTimeStamp()
00245     if now - self._startTime >= self.sampleRate:
00246         # If this Wiimote driver is to synchronize write
00247         # access to the wii state variable (which is read from
00248         # outside), then acquire the lock that was provided
00249         # by the instantiator of this instance:
00250         if self.wiiStateLock is not None:
00251             self.wiiStateLock.acquire()
00252         try:
00253           self.wiiMoteState = wiistate.WIIState(state, theTime, self.getRumble(), self._wm.state['buttons']);
00254         except ValueError:
00255           # A 'Wiimote is closed' error can occur as a race condition
00256           # as threads close down after a Cnt-C. Catch those and
00257           # ignore:
00258           pass
00259         if self.wiiStateLock is not None:
00260             self.wiiStateLock.release()
00261         self._startTime = now
00262 
00263   #----------------------------------------
00264   # _calibrationCallback
00265   #---------------------
00266 
00267   def _calibrationCallback(self, state, theTime):
00268     """Wii's callback destination while zeroing the device."""
00269 
00270     self._warmupCnt += 1
00271     if self._warmupCnt < NUM_WARMUP_READINGS:
00272         return
00273 
00274     if self._readingsCnt >= NUM_ZEROING_READINGS:
00275         return
00276 
00277     thisState = wiistate.WIIState(state, theTime, self.getRumble(), self._wm.state['buttons'])
00278 
00279     # Pull out the accelerometer x,y,z, accumulate in a list:
00280     self._accList.append(thisState.accRaw)
00281     
00282     # Pull out the gyro x,y,z, and build a GyroReading from them.
00283     # For a few cycles, the Wiimote does not deliver gyro info.
00284     # When it doesn't, we get a 'None' is unsubscriptable. Ignore 
00285     # those initial instabilities:
00286     try:
00287         self._gyroList.append(thisState.angleRate)
00288     except TypeError:
00289         pass
00290     self._readingsCnt += 1
00291 
00292     if thisState.nunchukPresent and self._nunchukJoyOrig is None:
00293         self._nunchukJoyOrig = thisState.nunchukStickRaw
00294         wiistate.WIIState.setNunchukJoystickCalibration(self._nunchukJoyOrig)
00295 
00296     return
00297 
00298   #----------------------------------------
00299   # zero
00300   #------------------
00301 
00302   def zeroDevice(self):
00303     """Find the at-rest values of the accelerometer and the gyro.
00304 
00305     Collect NUM_ZEROING_READINGS readings of acc and gyro. Average them.
00306     If the standard deviation of any of the six axes exceeds a threshold
00307     that was determined empirically, then the calibration fails. Else
00308     the gyro is biased to compensate for its at-rest offset. The offset
00309     is the abs(mean(Gyro)).
00310     
00311     The stdev thresholds are documented in wiimoteConstants.py.
00312     
00313     Note that we always use the Wiimote's factory-installed zeroing data.
00314     In the code below we nonetheless compute the stats for the 
00315     accelerometer, in case this behavior is to change in the future.
00316     
00317     We sleep while the samples are taken. In order to prevent other
00318     threads from reading bad values for mean/stdev, and variance, 
00319     we lock access to those vars.
00320     """
00321 
00322     self._accList = []    # Calibration callback will put samples here (WIIReading()s)
00323     self._gyroList = []   # Calibration callback will put samples here (WIIReading()s)
00324     self._readingsCnt = 0
00325     self._warmupCnt = 0
00326     # The factory calibration setting for the accelerometer (two values in a tuple):
00327     accCalibrationOrig = self.getAccelerometerCalibration()
00328     gyroCalibrationOrig = self.getGyroCalibration()
00329     accArrays = []        # Place to put raw reading triplets
00330     gyroArrays = []       # Place to put raw reading triplets
00331     
00332     try:
00333         # Get the samples for accelerometer and gyro:
00334         self.wiiStateLock.acquire()
00335 
00336         self._wiiCallbackStack.push(self._calibrationCallback)
00337         
00338         # Wipe out previous calibration correction data
00339         # while we gather raw samples:
00340         wiistate.WIIState.setGyroCalibration([0,0,0])
00341         wiistate.WIIState.setAccelerometerCalibration([0,0,0], [0,0,0])
00342                                                               
00343         while (self._readingsCnt < NUM_ZEROING_READINGS) or (self._warmupCnt < NUM_WARMUP_READINGS):
00344           time.sleep(.1)
00345          
00346         self._wiiCallbackStack.pause()
00347     finally:
00348         # Restore the callback that was in force before zeroing:
00349         self._wiiCallbackStack.pop()
00350         self.wiiStateLock.release()
00351 
00352     # Compute and store basic statistics about the readings:
00353     self.computeAccStatistics()
00354     self.computeGyroStatistics()
00355     
00356     # Extract the accelerometer reading triplets from the list of WIIReading()s:
00357     for accWiiReading in self._accList:
00358         if accWiiReading is not None:
00359             oneAccReading = accWiiReading.tuple()
00360             accArrays.append(oneAccReading)
00361     accArrays = np.reshape(accArrays, (-1,3))
00362     
00363     # Extract the gyro reading triplets from the list of WIIReading()s:
00364     if (self.motionPlusPresent()):     
00365         for gyroReading in self._gyroList:
00366             if (gyroReading is not None):
00367                 oneGyroReading = gyroReading.tuple()
00368                 gyroArrays.append(oneGyroReading)
00369     
00370     if (self.motionPlusPresent()):
00371         gyroArrays = np.reshape(gyroArrays, (-1,3))
00372         
00373         # We now have:
00374         # [[accX1, accZ1, accZ1]
00375         #  [accX2, accZ2, accZ2]
00376         #      ...
00377         #  ]
00378         # 
00379         # and:
00380         # 
00381         # [[gyroX1, gyroZ1, gyroZ1]
00382         #  [gyroX2, gyroZ2, gyroZ2]
00383         #      ...
00384         #  ]
00385         # 
00386         # Combine all into:
00387         # 
00388         # [[accX1, accZ1, accZ1, gyroX1, gyroZ1, gyroZ1]
00389         #  [accX2, accZ2, accZ2, gyroX2, gyroZ2, gyroZ2]
00390         #      ...
00391         #  ]
00392         # 
00393        
00394         allData = np.append(accArrays, gyroArrays, axis=1)
00395         # Will compare both, accelerometer x/y/z, and gyro x/y/z
00396         # to their stdev threshold to validate calibration:
00397         thresholdsArray = THRESHOLDS_ARRAY
00398     else:
00399         allData = accArrays
00400         # Will compare only accelerometer x/y/z to their stdev
00401         # threshold to validate calibration. No Wiimote+ was
00402         # detected:
00403         thresholdsArray = THRESHOLDS_ARRAY[0:3]
00404       
00405     # And take the std deviations column-wise:
00406     stdev = np.std(allData, axis=0)
00407     
00408     # See whether any of the six stdevs exceeds the
00409     # calibration threshold:
00410     
00411     isBadCalibration = (stdev > thresholdsArray).any()
00412 
00413     # We always use the factory-installed calibration info,
00414     self.setAccelerometerCalibration(accCalibrationOrig[0], accCalibrationOrig[1])
00415         
00416     if (isBadCalibration):
00417         self.latestCalibrationSuccessful = False;
00418         # We can calibrate the Wiimote anyway, if the preference
00419         # constant in wiimoteConstants.py is set accordingly:
00420         if (CALIBRATE_WITH_FAILED_CALIBRATION_DATA and self.motionPlusPresent()):
00421             rospy.loginfo("Failed calibration; using questionable calibration anyway.")
00422             wiistate.WIIState.setGyroCalibration(self.meanGyro)
00423         else:
00424             if (gyroCalibrationOrig is not None):
00425                 rospy.loginfo("Failed calibration; retaining previous calibration.")
00426                 if (self.motionPlusPresent()):
00427                     wiistate.WIIState.setGyroCalibration(gyroCalibrationOrig)
00428             else:
00429                 rospy.loginfo("Failed calibration; running without calibration now.")
00430         return False
00431     
00432     # Tell the WIIState factory that all WIIMote state instance creations
00433     # should correct accelerometer readings automatically, using the 
00434     # Nintendo-factory-set values: 
00435 
00436     
00437     # Do WIIState's gyro zero reading, so that future
00438     # readings can be corrected when a WIIState is created:
00439     wiistate.WIIState.setGyroCalibration(self.meanGyro)
00440             
00441     self.lastZeroingTime = getTimeStamp()
00442     rospy.loginfo("Calibration successful.")
00443     self.latestCalibrationSuccessful = True;
00444     return True;
00445 
00446   #----------------------------------------
00447   # getWiimoteState
00448   #------------------
00449 
00450   def getWiimoteState(self):
00451       """Returns the most recent Wiistate instance. Provides proper locking."""
00452       
00453       return self._getInstanceVarCriticalSection("wiimoteState")
00454   
00455   #----------------------------------------
00456   # getMeanAccelerator
00457   #------------------
00458 
00459   def getMeanAccelerator(self):
00460       """Accessor that provides locking."""
00461       
00462       return self._getInstanceVarCriticalSection("meanAcc")
00463   
00464   #----------------------------------------
00465   # getStdevAccelerator
00466   #------------------
00467 
00468   def getStdevAccelerator(self):
00469       """Accessor that provides locking."""
00470       
00471       return self._getInstanceVarCriticalSection("stdevAcc")
00472  
00473   #----------------------------------------
00474   # getVarianceAccelerator
00475   #------------------
00476 
00477   def getVarianceAccelerator(self):
00478       """Accessor that provides locking."""
00479       
00480       return self._getInstanceVarCriticalSection("varAcc")
00481 
00482   #----------------------------------------
00483   # getMeanGyro
00484   #------------------
00485 
00486   def getMeanGyro(self):
00487       """Accessor that provides locking."""
00488       
00489       return self._getInstanceVarCriticalSection("meanGyro")
00490   
00491   #----------------------------------------
00492   # getStdevGyro
00493   #------------------
00494 
00495   def getStdevGyro(self):
00496       """Accessor that provides locking."""
00497       
00498       return self._getInstanceVarCriticalSection("stdevGyro")
00499  
00500   #----------------------------------------
00501   # getVarianceGyro
00502   #------------------
00503 
00504   def getVarianceGyro(self):
00505       """Accessor that provides locking."""
00506       
00507       return self._getInstanceVarCriticalSection("varGyro")
00508 
00509  
00510   #----------------------------------------
00511   # _getInstanceVarCriticalSection
00512   #------------------
00513 
00514   def _getInstanceVarCriticalSection(self, varName):
00515       """Return the value of the given instance variable, providing locking service."""
00516       
00517       try: 
00518           self.wiiStateLock.acquire()
00519           
00520           if varName == "wiimoteState":
00521               res = self.wiiMoteState
00522           elif varName == "meanAcc":
00523               res = self.meanAcc
00524           elif varName == "stdevAcc":
00525               res = self.stdevAcc
00526           elif varName == "varAcc":
00527               res = self.varAcc
00528           elif varName == "meanGyro":
00529               res = self.meanGyro
00530           elif varName == "stdevGyro":
00531               res = self.stdevGyro
00532           elif varName == "varGyro":
00533               res = self.varGyroMetric
00534           else:
00535               raise ValueError("Instance variable name " + str(varName) + "is not under lock control." )
00536           
00537       finally:
00538           self.wiiStateLock.release()
00539           return res
00540  
00541   #----------------------------------------
00542   # setRumble
00543   #------------------
00544 
00545   def setRumble(self, switchPos):
00546     """Start of stop rumble (i.e. vibration). 1: start; 0: stop""" 
00547     self._wm.rumble = switchPos
00548 
00549 
00550   #----------------------------------------
00551   # getRumble
00552   #------------------
00553 
00554   def getRumble(self):
00555     # Protect against reading exception from reading
00556     # from an already closed device during shutdown:
00557     try:
00558       return self._wm.state['rumble']
00559     except ValueError:
00560       pass
00561 
00562   #----------------------------------------
00563   # setLEDs
00564   #------------------
00565 
00566   def setLEDs(self, statusList):
00567     """Set the four Wii LEDs according to statusList
00568 
00569     statusList must be a 4-tuple. Each entry
00570     is either True/1, False/0, or None. True (or 1) 
00571     will turn the respective LED on; False (or 0) 
00572     turns it off, and None leaves the state unchanged.
00573 
00574     """
00575 
00576     currLEDs = self.getLEDs(asInt=True)
00577     # Cycle through each LED:
00578     for LED in range(len(statusList)):
00579       # Should this LED be on?
00580       if statusList[LED]:
00581         currLEDs = currLEDs | self._LEDMasksOn[LED]
00582       # Is this LED to be OFF? (if not, leave it alone)
00583       elif statusList[LED] is not None:
00584         currLEDs = currLEDs & self._LEDMasksOff[LED]
00585     self._wm.led = currLEDs
00586         
00587 
00588   #----------------------------------------
00589   # getLEDs
00590   #------------------
00591 
00592   def getLEDs(self, asInt=False):
00593     """Get the status of the four Wii LEDs.
00594 
00595     Return value depends on the asInt parameter:
00596     if asInt=False, the method returns a 4-tuple. 
00597       Each entry is either True or False. True indicates
00598       that the respective LED is on; False means off.
00599     If asInt=True, return value is a bit vector
00600       indicating which LEDs are on.
00601 
00602     """
00603 
00604     LEDs = self._wm.state['led']
00605     if asInt:
00606       return LEDs
00607     res = []
00608     if LEDs & LED1_ON:
00609       res.append(True)
00610     else:
00611       res.append(False)
00612 
00613     if LEDs & LED2_ON:
00614       res.append(True)
00615     else:
00616       res.append(False)
00617 
00618     if LEDs & LED3_ON:
00619       res.append(True)
00620     else:
00621       res.append(False)
00622 
00623     if LEDs & LED4_ON:
00624       res.append(True)
00625     else:
00626       res.append(False)
00627 
00628     return res
00629 
00630   #----------------------------------------
00631   # getBattery
00632   #------------------
00633 
00634   def getBattery(self):
00635     """Obtain battery state from Wiimote.
00636 
00637     Maximum charge is BATTERY_MAX.
00638     """
00639 
00640     return self._wm.state['battery']
00641 
00642   #----------------------------------------
00643   # getAccelerometerCalibration
00644   #----------
00645   
00646   def getAccelerometerCalibration(self):
00647       """Returns currently operative accelerometer calibration.
00648       
00649       Return value: tuple with calibration for zero reading, and
00650       calibration or a '1' reading.
00651      """
00652       return wiistate.WIIState.getAccelerometerCalibration()
00653   
00654   #----------------------------------------
00655   # getAccFactoryCalibrationSettings
00656   #------------------
00657 
00658   def getAccFactoryCalibrationSettings(self):
00659     """Obtain calibration data from accelerometer.
00660 
00661     Retrieve factory-installed calibration data for
00662     the Wiimote's accelerometer. Returns a two-tuple
00663     with the calibration numbers for zero and one:
00664 
00665     """
00666 
00667     # Parameter is the Wiimote extension from which
00668     # the calibration is to be retrieved. 
00669 
00670     factoryCalNums = self._wm.get_acc_cal(cwiid.EXT_NONE);
00671 
00672     return (factoryCalNums[0], factoryCalNums[1])
00673 
00674   #----------------------------------------
00675   # getNunchukFactoryCalibrationSettings
00676   #------------------
00677 
00678   def getNunchukFactoryCalibrationSettings(self):
00679     """Obtain calibration data from nunchuk accelerometer.
00680 
00681     Retrieve factory-installed calibration data for
00682     the Nunchuk's accelerometer. Returns a two-tuple
00683     with the calibration numbers for zero and one:
00684 
00685     """
00686     factoryCalNums = self._wm.get_acc_cal(cwiid.EXT_NUNCHUK);
00687     return (factoryCalNums[0], factoryCalNums[1])
00688     
00689   #----------------------------------------
00690   # setAccelerometerCalibration
00691   #----------
00692   
00693   def setAccelerometerCalibration(self, zeroReadingList, oneReadingList):
00694       wiistate.WIIState.setAccelerometerCalibration(np.array(zeroReadingList), np.array(oneReadingList))
00695     
00696   def setAccelerometerCalibration(self, zeroReadingNPArray, oneReadingNPArray):
00697       wiistate.WIIState.setAccelerometerCalibration(zeroReadingNPArray, oneReadingNPArray)
00698 
00699   #----------------------------------------
00700   # getGyroCalibration
00701   #------------------
00702 
00703   def getGyroCalibration(self):
00704       """Return current Gyro zeroing offsets as list x/y/z."""
00705       return wiistate.WIIState.getGyroCalibration()
00706   
00707   #----------------------------------------
00708   # setGyroCalibration
00709   #------------------
00710 
00711   def setGyroCalibration(self, gyroTriplet):
00712       wiistate.WIIState.setGyroCalibration(gyroTriplet)
00713 
00714   #----------------------------------------
00715   # setNunchukAccelerometerCalibration
00716   #----------
00717   
00718   def setNunchukAccelerometerCalibration(self, zeroReadingList, oneReadingList):
00719       wiistate.WIIState.setNunchukAccelerometerCalibration(np.array(zeroReadingList), np.array(oneReadingList))
00720     
00721   #----------------------------------------
00722   # motionPlusPresent
00723   #------------------
00724 
00725   def motionPlusPresent(self):
00726       """Return True/False to indicate whether a Wiimotion Plus is detected.
00727       
00728       Note: The return value is accurate only after at least one 
00729       Wiimote state has been read. This means that either 
00730       _steadyStateCallback or _calibrationCallback must have
00731       run at least once.
00732       """
00733       if (self.wiiMoteState is not None):
00734           return self.wiiMoteState.motionPlusPresent
00735       else:
00736           return False
00737 
00738   #----------------------------------------
00739   # nunchukPresent
00740   #------------------
00741 
00742   def nunchukPresent(self):
00743       """Return True/False to indicate whether a Nunchuk is detected.
00744       
00745       Note: The return value is accurate only after at least one 
00746       Wiimote state has been read. This means that either 
00747       _steadyStateCallback or _calibrationCallback must have
00748       run at least once.
00749       """
00750       if (self.wiiMoteState is not None):
00751           return self.wiiMoteState.nunchukPresent
00752       else:
00753           return False
00754     
00755   #----------------------------------------
00756   # computeAccStatistics
00757   #------------------
00758  
00759   def computeAccStatistics(self):
00760       """Compute mean and stdev for accelerometer data list self._accList in both Gs and metric m/sec^2"""
00761 
00762       accArrays = []
00763       self.maxAccReading = np.array([0,0,0], dtype=None, copy=1, order=None, subok=0, ndmin=0)
00764       for accWiiReading in self._accList:
00765           if accWiiReading is not None:
00766               oneAccReading = accWiiReading.tuple()
00767               accArrays.append(oneAccReading)
00768               self.maxAccReading = np.maximum(self.maxAccReading, np.abs(oneAccReading))
00769         
00770       # Turn list of numpy triplets into three columns containing
00771       # all x, all y, and all z values, respectively:
00772       #  [array(10,20,30), array(100,200,300)] ==> [[10   20  30],
00773       #                                             [100 200 300]]
00774       # and take the means of each column. We will end up
00775       # with: [55.0 110.0 165.0]
00776       
00777       self.meanAcc = np.vstack(accArrays).mean(axis=0)
00778       self.meanAccMetric = self.meanAcc * EARTH_GRAVITY
00779       self.stdevAcc = np.vstack(accArrays).std(axis=0)
00780       self.stdevAccMetric = self.stdevAcc * EARTH_GRAVITY
00781       self.varAcc = np.square(self.stdevAccMetric)
00782 
00783 
00784   #----------------------------------------
00785   # computeGyroStatistics
00786   #------------------
00787   
00788   def computeGyroStatistics(self):
00789       """Compute mean and stdev for gyro data list self._gyroList in both Gs and metric m/sec^2"""      
00790       gyroArrays = []
00791       self.maxGyroReading = np.array([0,0,0],dtype=np.float64)        
00792       for gyroReading in self._gyroList:
00793           if (gyroReading is not None):
00794               oneGyroReading = gyroReading.tuple()
00795               gyroArrays.append(oneGyroReading)
00796               self.maxGyroReading = np.maximum(self.maxGyroReading, np.abs(oneGyroReading))
00797             
00798       if len(gyroArrays) != 0:
00799           self.meanGyro = np.vstack(gyroArrays).mean(axis=0)
00800           # Convert to radians/sec:
00801           self.meanGyroMetric = self.meanGyro * GYRO_SCALE_FACTOR
00802           self.stdevGyro = np.vstack(gyroArrays).std(axis=0)
00803           # Convert stdev to radians/sec:
00804           self.stdevGyroMetric = self.stdevGyro * GYRO_SCALE_FACTOR
00805           self.varGyroMetric = np.square(self.stdevGyroMetric)
00806 
00807     
00808   #----------------------------------------
00809   # printState
00810   #------------------
00811 
00812   def printState(self):
00813       log(self.wiiMoteState)
00814     
00815          
00816   #----------------------------------------
00817   # shutdown
00818   #------------------
00819 
00820   def shutdown(self):
00821     self._wm.close()
00822 
00823 #;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
00824 #
00825 #    Class WiiCallbackStack
00826 #
00827 #;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
00828 
00829 
00830 class _WiiCallbackStack(object):
00831   """Class organizes installation and removal/restoration
00832   of callback functions for the Wii driver to use. 
00833   Only one instance of this class is allowed. Additional
00834   instantiations generate a CallbackStackMultInstError.
00835 
00836   A stack discipline is imposed. Operations:
00837 
00838      - push(<callBackFunc>)        # New function becomes the active
00839                                    # callback immediately
00840      - pop() -> <oldCallBackFunc>  # If another function is still on
00841                                        # the stack, it immediately becomes
00842                                        # the active callback. If callback
00843                                      # is paused, resume() is forced.
00844      - pause()                  # Callbacks are temporarily turned off
00845      - paused() -> True/False
00846      - resume(sloppy=True)      # If sloppy=False, resuming when
00847                                      # callbacks are not paused throws an
00848                                      # exception.  If sloppy=True, the call is
00849                                      # a no-op
00850 
00851   """
00852 
00853   _functionStack = []
00854   _singletonInstance = None  # No instance exists yet.
00855   _paused = False
00856 
00857   _wm = None                 # The Wii remote driver instance
00858 
00859 
00860   #----------------------------------------
00861   # __init__
00862   #------------------
00863 
00864   def __init__(self, wiiDriver, sloppy=True):
00865 
00866     if self._singletonInstance:
00867       if not sloppy:
00868         raise CallbackStackMultInstError("Can only instantiate one Wii callback stack.")
00869 
00870     self._singletonInstance = self
00871     self._wm = wiiDriver
00872 
00873   #----------------------------------------
00874   # push
00875   #------------------
00876 
00877   def push(self, func):
00878     """Given function becomes the new WIImote callback function, shadowing 
00879     the function that is currently on the stack
00880     """
00881     
00882     self._functionStack.append(func)
00883     self.setcallback(func)
00884 
00885   #----------------------------------------
00886   # pop
00887   #------------------
00888 
00889   def pop(self):
00890     """Wiimote callback function is popped off the stack. New top of stack 
00891     becomes the new callback function. Old function is returned.
00892     """
00893     
00894     if not self._functionStack:
00895       raise CallbackStackEmptyError("Attempt to pop empty callback stack")
00896     _paused = False
00897     func = self._functionStack.pop()
00898     self.setcallback(self._functionStack[-1])
00899     return func
00900 
00901   #----------------------------------------
00902   # pause
00903   #------------------
00904 
00905   def pause(self):
00906     """WIIMote callbacks are temporarily stopped."""
00907     
00908     self._wm.disable(cwiid.FLAG_MESG_IFC)    
00909     self._paused = True
00910     
00911   #----------------------------------------
00912   # resume
00913   #------------------
00914 
00915   def resume(self, sloppy=True):
00916     """Resume the (presumably) previously paused WIIMote callback functions.
00917     If sloppy is True, this method won't complain if pause was not 
00918     called earlier. If sloppy is False, an exception is raised in 
00919     that case.
00920     """
00921      
00922     if not self._paused:
00923       if sloppy:
00924         return
00925       else:
00926         raise ResumeNonPausedError("Can't resume without first pausing.")
00927     
00928     if not self._functionStack:
00929       raise CallbackStackEmptyError("Attempt to pop empty callback stack")
00930 
00931     self._wiiCallbackStack(_functionStack.index[-1])
00932 
00933 
00934   #----------------------------------------
00935   # setcallback
00936   #------------------
00937 
00938   def setcallback(self, f):
00939     """Tell WIIMote which function to call when reporting status."""
00940     
00941     self._wm.mesg_callback = f
00942     self._wm.enable(cwiid.FLAG_MESG_IFC)
00943 
00944       
00945 class CalibrationMeasurements():
00946     
00947     def __init__(self):
00948                  # runNum, meanAcc, maxAcc, stdevAcc, meanGyro, maxGyro, stdevGyro,
00949                  # accVal, devAccVal, stdevFractionAccVal, isOutlierAcc,
00950                  # gyroVal, devGyroVal, stdevFractionGyroVal, isOutlierGyro):
00951 
00952       pass
00953              
00954     def setAccData(self, accArray):
00955         self.accVal = accArray
00956     
00957     def setStdevAcc(self, stdevArray):
00958         self.stdevAcc = stdevArray
00959         
00960     def setMeanAcc(self, meanArray):
00961         self.meanAcc = meanArray
00962         
00963     def setMaxAcc(self, maxArray):
00964         self.maxAcc = maxArray
00965 
00966 
00967     def setGyroData(self, gyroArray):
00968         self.gyroVal = gyroArray
00969         
00970     def setStdevGyro(self, stdevArray):
00971         self.stdevGyro = stdevArray
00972         
00973     def setMeanGyro(self, meanArray):
00974         self.meanGyro = meanArray
00975 
00976     def setMaxGyro(self, maxArray):
00977         self.maxGyro = maxArray
00978       
00979     def setGyroData(self, gyroVal):
00980       self.gyroVal = gyroVal
00981                  


wiimote
Author(s): Andreas Paepcke, Melonee Wise
autogenerated on Mon Oct 6 2014 01:06:37