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     if (self._wm.state['ext_type'] == cwiid.EXT_NUNCHUK):
00222       try:
00223         (factoryZero, factoryOne) = self.getNunchukFactoryCalibrationSettings()
00224         self.setNunchukAccelerometerCalibration(factoryZero, factoryOne)
00225       except:
00226         pass
00227 
00228     time.sleep(0.2)
00229     self._wiiCallbackStack.push(self._steadyStateCallback)
00230 
00231     rospy.loginfo("Wiimote activated.")
00232 
00233 
00234   #----------------------------------------
00235   # steadyStateCallback
00236   #------------------
00237 
00238   def _steadyStateCallback(self, state, theTime):
00239     #print state
00240     now = getTimeStamp()
00241     if now - self._startTime >= self.sampleRate:
00242         # If this Wiimote driver is to synchronize write
00243         # access to the wii state variable (which is read from
00244         # outside), then acquire the lock that was provided
00245         # by the instantiator of this instance:
00246         if self.wiiStateLock is not None:
00247             self.wiiStateLock.acquire()
00248         try:
00249           self.wiiMoteState = wiistate.WIIState(state, theTime, self.getRumble(), self._wm.state['buttons']);
00250         except ValueError:
00251           # A 'Wiimote is closed' error can occur as a race condition
00252           # as threads close down after a Cnt-C. Catch those and
00253           # ignore:
00254           pass
00255         if self.wiiStateLock is not None:
00256             self.wiiStateLock.release()
00257         self._startTime = now
00258 
00259   #----------------------------------------
00260   # _calibrationCallback
00261   #---------------------
00262 
00263   def _calibrationCallback(self, state, theTime):
00264     """Wii's callback destination while zeroing the device."""
00265 
00266     self._warmupCnt += 1
00267     if self._warmupCnt < NUM_WARMUP_READINGS:
00268         return
00269 
00270     if self._readingsCnt >= NUM_ZEROING_READINGS:
00271         return
00272 
00273     thisState = wiistate.WIIState(state, theTime, self.getRumble(), self._wm.state['buttons'])
00274 
00275     # Pull out the accelerometer x,y,z, accumulate in a list:
00276     self._accList.append(thisState.accRaw)
00277     
00278     # Pull out the gyro x,y,z, and build a GyroReading from them.
00279     # For a few cycles, the Wiimote does not deliver gyro info.
00280     # When it doesn't, we get a 'None' is unsubscriptable. Ignore 
00281     # those initial instabilities:
00282     try:
00283         self._gyroList.append(thisState.angleRate)
00284     except TypeError:
00285         pass
00286     self._readingsCnt += 1
00287 
00288     if thisState.nunchukPresent and self._nunchukJoyOrig is None:
00289         self._nunchukJoyOrig = thisState.nunchukStickRaw
00290         wiistate.WIIState.setNunchukJoystickCalibration(self._nunchukJoyOrig)
00291 
00292     return
00293 
00294   #----------------------------------------
00295   # zero
00296   #------------------
00297 
00298   def zeroDevice(self):
00299     """Find the at-rest values of the accelerometer and the gyro.
00300 
00301     Collect NUM_ZEROING_READINGS readings of acc and gyro. Average them.
00302     If the standard deviation of any of the six axes exceeds a threshold
00303     that was determined empirically, then the calibration fails. Else
00304     the gyro is biased to compensate for its at-rest offset. The offset
00305     is the abs(mean(Gyro)).
00306     
00307     The stdev thresholds are documented in wiimoteConstants.py.
00308     
00309     Note that we always use the Wiimote's factory-installed zeroing data.
00310     In the code below we nonetheless compute the stats for the 
00311     accelerometer, in case this behavior is to change in the future.
00312     
00313     We sleep while the samples are taken. In order to prevent other
00314     threads from reading bad values for mean/stdev, and variance, 
00315     we lock access to those vars.
00316     """
00317 
00318     self._accList = []    # Calibration callback will put samples here (WIIReading()s)
00319     self._gyroList = []   # Calibration callback will put samples here (WIIReading()s)
00320     self._readingsCnt = 0
00321     self._warmupCnt = 0
00322     # The factory calibration setting for the accelerometer (two values in a tuple):
00323     accCalibrationOrig = self.getAccelerometerCalibration()
00324     gyroCalibrationOrig = self.getGyroCalibration()
00325     accArrays = []        # Place to put raw reading triplets
00326     gyroArrays = []       # Place to put raw reading triplets
00327     
00328     try:
00329         # Get the samples for accelerometer and gyro:
00330         self.wiiStateLock.acquire()
00331 
00332         self._wiiCallbackStack.push(self._calibrationCallback)
00333         
00334         # Wipe out previous calibration correction data
00335         # while we gather raw samples:
00336         wiistate.WIIState.setGyroCalibration([0,0,0])
00337         wiistate.WIIState.setAccelerometerCalibration([0,0,0], [0,0,0])
00338                                                               
00339         while (self._readingsCnt < NUM_ZEROING_READINGS) or (self._warmupCnt < NUM_WARMUP_READINGS):
00340           time.sleep(.1)
00341          
00342         self._wiiCallbackStack.pause()
00343     finally:
00344         # Restore the callback that was in force before zeroing:
00345         self._wiiCallbackStack.pop()
00346         self.wiiStateLock.release()
00347 
00348     # Compute and store basic statistics about the readings:
00349     self.computeAccStatistics()
00350     self.computeGyroStatistics()
00351     
00352     # Extract the accelerometer reading triplets from the list of WIIReading()s:
00353     for accWiiReading in self._accList:
00354         if accWiiReading is not None:
00355             oneAccReading = accWiiReading.tuple()
00356             accArrays.append(oneAccReading)
00357     accArrays = np.reshape(accArrays, (-1,3))
00358     
00359     # Extract the gyro reading triplets from the list of WIIReading()s:
00360     if (self.motionPlusPresent()):     
00361         for gyroReading in self._gyroList:
00362             if (gyroReading is not None):
00363                 oneGyroReading = gyroReading.tuple()
00364                 gyroArrays.append(oneGyroReading)
00365     
00366     if (self.motionPlusPresent()):
00367         gyroArrays = np.reshape(gyroArrays, (-1,3))
00368         
00369         # We now have:
00370         # [[accX1, accZ1, accZ1]
00371         #  [accX2, accZ2, accZ2]
00372         #      ...
00373         #  ]
00374         # 
00375         # and:
00376         # 
00377         # [[gyroX1, gyroZ1, gyroZ1]
00378         #  [gyroX2, gyroZ2, gyroZ2]
00379         #      ...
00380         #  ]
00381         # 
00382         # Combine all into:
00383         # 
00384         # [[accX1, accZ1, accZ1, gyroX1, gyroZ1, gyroZ1]
00385         #  [accX2, accZ2, accZ2, gyroX2, gyroZ2, gyroZ2]
00386         #      ...
00387         #  ]
00388         # 
00389        
00390         allData = np.append(accArrays, gyroArrays, axis=1)
00391         # Will compare both, accelerometer x/y/z, and gyro x/y/z
00392         # to their stdev threshold to validate calibration:
00393         thresholdsArray = THRESHOLDS_ARRAY
00394     else:
00395         allData = accArrays
00396         # Will compare only accelerometer x/y/z to their stdev
00397         # threshold to validate calibration. No Wiimote+ was
00398         # detected:
00399         thresholdsArray = THRESHOLDS_ARRAY[0:3]
00400       
00401     # And take the std deviations column-wise:
00402     stdev = np.std(allData, axis=0)
00403     
00404     # See whether any of the six stdevs exceeds the
00405     # calibration threshold:
00406     
00407     isBadCalibration = (stdev > thresholdsArray).any()
00408 
00409     # We always use the factory-installed calibration info,
00410     self.setAccelerometerCalibration(accCalibrationOrig[0], accCalibrationOrig[1])
00411         
00412     if (isBadCalibration):
00413         self.latestCalibrationSuccessful = False;
00414         # We can calibrate the Wiimote anyway, if the preference
00415         # constant in wiimoteConstants.py is set accordingly:
00416         if (CALIBRATE_WITH_FAILED_CALIBRATION_DATA and self.motionPlusPresent()):
00417             rospy.loginfo("Failed calibration; using questionable calibration anyway.")
00418             wiistate.WIIState.setGyroCalibration(self.meanGyro)
00419         else:
00420             if (gyroCalibrationOrig is not None):
00421                 rospy.loginfo("Failed calibration; retaining previous calibration.")
00422                 if (self.motionPlusPresent()):
00423                     wiistate.WIIState.setGyroCalibration(gyroCalibrationOrig)
00424             else:
00425                 rospy.loginfo("Failed calibration; running without calibration now.")
00426         return False
00427     
00428     # Tell the WIIState factory that all WIIMote state instance creations
00429     # should correct accelerometer readings automatically, using the 
00430     # Nintendo-factory-set values: 
00431 
00432     
00433     # Do WIIState's gyro zero reading, so that future
00434     # readings can be corrected when a WIIState is created:
00435     wiistate.WIIState.setGyroCalibration(self.meanGyro)
00436             
00437     self.lastZeroingTime = getTimeStamp()
00438     rospy.loginfo("Calibration successful.")
00439     self.latestCalibrationSuccessful = True;
00440     return True;
00441 
00442   #----------------------------------------
00443   # getWiimoteState
00444   #------------------
00445 
00446   def getWiimoteState(self):
00447       """Returns the most recent Wiistate instance. Provides proper locking."""
00448       
00449       return self._getInstanceVarCriticalSection("wiimoteState")
00450   
00451   #----------------------------------------
00452   # getMeanAccelerator
00453   #------------------
00454 
00455   def getMeanAccelerator(self):
00456       """Accessor that provides locking."""
00457       
00458       return self._getInstanceVarCriticalSection("meanAcc")
00459   
00460   #----------------------------------------
00461   # getStdevAccelerator
00462   #------------------
00463 
00464   def getStdevAccelerator(self):
00465       """Accessor that provides locking."""
00466       
00467       return self._getInstanceVarCriticalSection("stdevAcc")
00468  
00469   #----------------------------------------
00470   # getVarianceAccelerator
00471   #------------------
00472 
00473   def getVarianceAccelerator(self):
00474       """Accessor that provides locking."""
00475       
00476       return self._getInstanceVarCriticalSection("varAcc")
00477 
00478   #----------------------------------------
00479   # getMeanGyro
00480   #------------------
00481 
00482   def getMeanGyro(self):
00483       """Accessor that provides locking."""
00484       
00485       return self._getInstanceVarCriticalSection("meanGyro")
00486   
00487   #----------------------------------------
00488   # getStdevGyro
00489   #------------------
00490 
00491   def getStdevGyro(self):
00492       """Accessor that provides locking."""
00493       
00494       return self._getInstanceVarCriticalSection("stdevGyro")
00495  
00496   #----------------------------------------
00497   # getVarianceGyro
00498   #------------------
00499 
00500   def getVarianceGyro(self):
00501       """Accessor that provides locking."""
00502       
00503       return self._getInstanceVarCriticalSection("varGyro")
00504 
00505  
00506   #----------------------------------------
00507   # _getInstanceVarCriticalSection
00508   #------------------
00509 
00510   def _getInstanceVarCriticalSection(self, varName):
00511       """Return the value of the given instance variable, providing locking service."""
00512       
00513       try: 
00514           self.wiiStateLock.acquire()
00515           
00516           if varName == "wiimoteState":
00517               res = self.wiiMoteState
00518           elif varName == "meanAcc":
00519               res = self.meanAcc
00520           elif varName == "stdevAcc":
00521               res = self.stdevAcc
00522           elif varName == "varAcc":
00523               res = self.varAcc
00524           elif varName == "meanGyro":
00525               res = self.meanGyro
00526           elif varName == "stdevGyro":
00527               res = self.stdevGyro
00528           elif varName == "varGyro":
00529               res = self.varGyroMetric
00530           else:
00531               raise ValueError("Instance variable name " + str(varName) + "is not under lock control." )
00532           
00533       finally:
00534           self.wiiStateLock.release()
00535           return res
00536  
00537   #----------------------------------------
00538   # setRumble
00539   #------------------
00540 
00541   def setRumble(self, switchPos):
00542     """Start of stop rumble (i.e. vibration). 1: start; 0: stop""" 
00543     self._wm.rumble = switchPos
00544 
00545 
00546   #----------------------------------------
00547   # getRumble
00548   #------------------
00549 
00550   def getRumble(self):
00551     # Protect against reading exception from reading
00552     # from an already closed device during shutdown:
00553     try:
00554       return self._wm.state['rumble']
00555     except ValueError:
00556       pass
00557 
00558   #----------------------------------------
00559   # setLEDs
00560   #------------------
00561 
00562   def setLEDs(self, statusList):
00563     """Set the four Wii LEDs according to statusList
00564 
00565     statusList must be a 4-tuple. Each entry
00566     is either True/1, False/0, or None. True (or 1) 
00567     will turn the respective LED on; False (or 0) 
00568     turns it off, and None leaves the state unchanged.
00569 
00570     """
00571 
00572     currLEDs = self.getLEDs(asInt=True)
00573     # Cycle through each LED:
00574     for LED in range(len(statusList)):
00575       # Should this LED be on?
00576       if statusList[LED]:
00577         currLEDs = currLEDs | self._LEDMasksOn[LED]
00578       # Is this LED to be OFF? (if not, leave it alone)
00579       elif statusList[LED] is not None:
00580         currLEDs = currLEDs & self._LEDMasksOff[LED]
00581     self._wm.led = currLEDs
00582         
00583 
00584   #----------------------------------------
00585   # getLEDs
00586   #------------------
00587 
00588   def getLEDs(self, asInt=False):
00589     """Get the status of the four Wii LEDs.
00590 
00591     Return value depends on the asInt parameter:
00592     if asInt=False, the method returns a 4-tuple. 
00593       Each entry is either True or False. True indicates
00594       that the respective LED is on; False means off.
00595     If asInt=True, return value is a bit vector
00596       indicating which LEDs are on.
00597 
00598     """
00599 
00600     LEDs = self._wm.state['led']
00601     if asInt:
00602       return LEDs
00603     res = []
00604     if LEDs & LED1_ON:
00605       res.append(True)
00606     else:
00607       res.append(False)
00608 
00609     if LEDs & LED2_ON:
00610       res.append(True)
00611     else:
00612       res.append(False)
00613 
00614     if LEDs & LED3_ON:
00615       res.append(True)
00616     else:
00617       res.append(False)
00618 
00619     if LEDs & LED4_ON:
00620       res.append(True)
00621     else:
00622       res.append(False)
00623 
00624     return res
00625 
00626   #----------------------------------------
00627   # getBattery
00628   #------------------
00629 
00630   def getBattery(self):
00631     """Obtain battery state from Wiimote.
00632 
00633     Maximum charge is BATTERY_MAX.
00634     """
00635 
00636     return self._wm.state['battery']
00637 
00638   #----------------------------------------
00639   # getAccelerometerCalibration
00640   #----------
00641   
00642   def getAccelerometerCalibration(self):
00643       """Returns currently operative accelerometer calibration.
00644       
00645       Return value: tuple with calibration for zero reading, and
00646       calibration or a '1' reading.
00647      """
00648       return wiistate.WIIState.getAccelerometerCalibration()
00649   
00650   #----------------------------------------
00651   # getAccFactoryCalibrationSettings
00652   #------------------
00653 
00654   def getAccFactoryCalibrationSettings(self):
00655     """Obtain calibration data from accelerometer.
00656 
00657     Retrieve factory-installed calibration data for
00658     the Wiimote's accelerometer. Returns a two-tuple
00659     with the calibration numbers for zero and one:
00660 
00661     """
00662 
00663     # Parameter is the Wiimote extension from which
00664     # the calibration is to be retrieved. 
00665 
00666     factoryCalNums = self._wm.get_acc_cal(cwiid.EXT_NONE);
00667 
00668     return (factoryCalNums[0], factoryCalNums[1])
00669 
00670   #----------------------------------------
00671   # getNunchukFactoryCalibrationSettings
00672   #------------------
00673 
00674   def getNunchukFactoryCalibrationSettings(self):
00675     """Obtain calibration data from nunchuk accelerometer.
00676 
00677     Retrieve factory-installed calibration data for
00678     the Nunchuk's accelerometer. Returns a two-tuple
00679     with the calibration numbers for zero and one:
00680 
00681     """
00682     factoryCalNums = self._wm.get_acc_cal(cwiid.EXT_NUNCHUK);
00683     return (factoryCalNums[0], factoryCalNums[1])
00684     
00685   #----------------------------------------
00686   # setAccelerometerCalibration
00687   #----------
00688   
00689   def setAccelerometerCalibration(self, zeroReadingList, oneReadingList):
00690       wiistate.WIIState.setAccelerometerCalibration(np.array(zeroReadingList), np.array(oneReadingList))
00691     
00692   def setAccelerometerCalibration(self, zeroReadingNPArray, oneReadingNPArray):
00693       wiistate.WIIState.setAccelerometerCalibration(zeroReadingNPArray, oneReadingNPArray)
00694 
00695   #----------------------------------------
00696   # getGyroCalibration
00697   #------------------
00698 
00699   def getGyroCalibration(self):
00700       """Return current Gyro zeroing offsets as list x/y/z."""
00701       return wiistate.WIIState.getGyroCalibration()
00702   
00703   #----------------------------------------
00704   # setGyroCalibration
00705   #------------------
00706 
00707   def setGyroCalibration(self, gyroTriplet):
00708       wiistate.WIIState.setGyroCalibration(gyroTriplet)
00709 
00710   #----------------------------------------
00711   # setNunchukAccelerometerCalibration
00712   #----------
00713   
00714   def setNunchukAccelerometerCalibration(self, zeroReadingList, oneReadingList):
00715       wiistate.WIIState.setNunchukAccelerometerCalibration(np.array(zeroReadingList), np.array(oneReadingList))
00716     
00717   #----------------------------------------
00718   # motionPlusPresent
00719   #------------------
00720 
00721   def motionPlusPresent(self):
00722       """Return True/False to indicate whether a Wiimotion Plus is detected.
00723       
00724       Note: The return value is accurate only after at least one 
00725       Wiimote state has been read. This means that either 
00726       _steadyStateCallback or _calibrationCallback must have
00727       run at least once.
00728       """
00729       if (self.wiiMoteState is not None):
00730           return self.wiiMoteState.motionPlusPresent
00731       else:
00732           return False
00733 
00734   #----------------------------------------
00735   # nunchukPresent
00736   #------------------
00737 
00738   def nunchukPresent(self):
00739       """Return True/False to indicate whether a Nunchuk is detected.
00740       
00741       Note: The return value is accurate only after at least one 
00742       Wiimote state has been read. This means that either 
00743       _steadyStateCallback or _calibrationCallback must have
00744       run at least once.
00745       """
00746       if (self.wiiMoteState is not None):
00747           return self.wiiMoteState.nunchukPresent
00748       else:
00749           return False
00750     
00751   #----------------------------------------
00752   # computeAccStatistics
00753   #------------------
00754  
00755   def computeAccStatistics(self):
00756       """Compute mean and stdev for accelerometer data list self._accList in both Gs and metric m/sec^2"""
00757 
00758       accArrays = []
00759       self.maxAccReading = np.array([0,0,0], dtype=None, copy=1, order=None, subok=0, ndmin=0)
00760       for accWiiReading in self._accList:
00761           if accWiiReading is not None:
00762               oneAccReading = accWiiReading.tuple()
00763               accArrays.append(oneAccReading)
00764               self.maxAccReading = np.maximum(self.maxAccReading, np.abs(oneAccReading))
00765         
00766       # Turn list of numpy triplets into three columns containing
00767       # all x, all y, and all z values, respectively:
00768       #  [array(10,20,30), array(100,200,300)] ==> [[10   20  30],
00769       #                                             [100 200 300]]
00770       # and take the means of each column. We will end up
00771       # with: [55.0 110.0 165.0]
00772       
00773       self.meanAcc = np.vstack(accArrays).mean(axis=0)
00774       self.meanAccMetric = self.meanAcc * EARTH_GRAVITY
00775       self.stdevAcc = np.vstack(accArrays).std(axis=0)
00776       self.stdevAccMetric = self.stdevAcc * EARTH_GRAVITY
00777       self.varAcc = np.square(self.stdevAccMetric)
00778 
00779 
00780   #----------------------------------------
00781   # computeGyroStatistics
00782   #------------------
00783   
00784   def computeGyroStatistics(self):
00785       """Compute mean and stdev for gyro data list self._gyroList in both Gs and metric m/sec^2"""      
00786       gyroArrays = []
00787       self.maxGyroReading = np.array([0,0,0],dtype=np.float64)        
00788       for gyroReading in self._gyroList:
00789           if (gyroReading is not None):
00790               oneGyroReading = gyroReading.tuple()
00791               gyroArrays.append(oneGyroReading)
00792               self.maxGyroReading = np.maximum(self.maxGyroReading, np.abs(oneGyroReading))
00793             
00794       if len(gyroArrays) != 0:
00795           self.meanGyro = np.vstack(gyroArrays).mean(axis=0)
00796           # Convert to radians/sec:
00797           self.meanGyroMetric = self.meanGyro * GYRO_SCALE_FACTOR
00798           self.stdevGyro = np.vstack(gyroArrays).std(axis=0)
00799           # Convert stdev to radians/sec:
00800           self.stdevGyroMetric = self.stdevGyro * GYRO_SCALE_FACTOR
00801           self.varGyroMetric = np.square(self.stdevGyroMetric)
00802 
00803     
00804   #----------------------------------------
00805   # printState
00806   #------------------
00807 
00808   def printState(self):
00809       log(self.wiiMoteState)
00810     
00811          
00812   #----------------------------------------
00813   # shutdown
00814   #------------------
00815 
00816   def shutdown(self):
00817     self._wm.close()
00818 
00819 #;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
00820 #
00821 #    Class WiiCallbackStack
00822 #
00823 #;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
00824 
00825 
00826 class _WiiCallbackStack(object):
00827   """Class organizes installation and removal/restoration
00828   of callback functions for the Wii driver to use. 
00829   Only one instance of this class is allowed. Additional
00830   instantiations generate a CallbackStackMultInstError.
00831 
00832   A stack discipline is imposed. Operations:
00833 
00834      - push(<callBackFunc>)        # New function becomes the active
00835                                    # callback immediately
00836      - pop() -> <oldCallBackFunc>  # If another function is still on
00837                                        # the stack, it immediately becomes
00838                                        # the active callback. If callback
00839                                      # is paused, resume() is forced.
00840      - pause()                  # Callbacks are temporarily turned off
00841      - paused() -> True/False
00842      - resume(sloppy=True)      # If sloppy=False, resuming when
00843                                      # callbacks are not paused throws an
00844                                      # exception.  If sloppy=True, the call is
00845                                      # a no-op
00846 
00847   """
00848 
00849   _functionStack = []
00850   _singletonInstance = None  # No instance exists yet.
00851   _paused = False
00852 
00853   _wm = None                 # The Wii remote driver instance
00854 
00855 
00856   #----------------------------------------
00857   # __init__
00858   #------------------
00859 
00860   def __init__(self, wiiDriver, sloppy=True):
00861 
00862     if self._singletonInstance:
00863       if not sloppy:
00864         raise CallbackStackMultInstError("Can only instantiate one Wii callback stack.")
00865 
00866     self._singletonInstance = self
00867     self._wm = wiiDriver
00868 
00869   #----------------------------------------
00870   # push
00871   #------------------
00872 
00873   def push(self, func):
00874     """Given function becomes the new WIImote callback function, shadowing 
00875     the function that is currently on the stack
00876     """
00877     
00878     self._functionStack.append(func)
00879     self.setcallback(func)
00880 
00881   #----------------------------------------
00882   # pop
00883   #------------------
00884 
00885   def pop(self):
00886     """Wiimote callback function is popped off the stack. New top of stack 
00887     becomes the new callback function. Old function is returned.
00888     """
00889     
00890     if not self._functionStack:
00891       raise CallbackStackEmptyError("Attempt to pop empty callback stack")
00892     _paused = False
00893     func = self._functionStack.pop()
00894     self.setcallback(self._functionStack[-1])
00895     return func
00896 
00897   #----------------------------------------
00898   # pause
00899   #------------------
00900 
00901   def pause(self):
00902     """WIIMote callbacks are temporarily stopped."""
00903     
00904     self._wm.disable(cwiid.FLAG_MESG_IFC)    
00905     self._paused = True
00906     
00907   #----------------------------------------
00908   # resume
00909   #------------------
00910 
00911   def resume(self, sloppy=True):
00912     """Resume the (presumably) previously paused WIIMote callback functions.
00913     If sloppy is True, this method won't complain if pause was not 
00914     called earlier. If sloppy is False, an exception is raised in 
00915     that case.
00916     """
00917      
00918     if not self._paused:
00919       if sloppy:
00920         return
00921       else:
00922         raise ResumeNonPausedError("Can't resume without first pausing.")
00923     
00924     if not self._functionStack:
00925       raise CallbackStackEmptyError("Attempt to pop empty callback stack")
00926 
00927     self._wiiCallbackStack(_functionStack.index[-1])
00928 
00929 
00930   #----------------------------------------
00931   # setcallback
00932   #------------------
00933 
00934   def setcallback(self, f):
00935     """Tell WIIMote which function to call when reporting status."""
00936     
00937     self._wm.mesg_callback = f
00938     self._wm.enable(cwiid.FLAG_MESG_IFC)
00939 
00940       
00941 class CalibrationMeasurements():
00942     
00943     def __init__(self):
00944                  # runNum, meanAcc, maxAcc, stdevAcc, meanGyro, maxGyro, stdevGyro,
00945                  # accVal, devAccVal, stdevFractionAccVal, isOutlierAcc,
00946                  # gyroVal, devGyroVal, stdevFractionGyroVal, isOutlierGyro):
00947 
00948       pass
00949              
00950     def setAccData(self, accArray):
00951         self.accVal = accArray
00952     
00953     def setStdevAcc(self, stdevArray):
00954         self.stdevAcc = stdevArray
00955         
00956     def setMeanAcc(self, meanArray):
00957         self.meanAcc = meanArray
00958         
00959     def setMaxAcc(self, maxArray):
00960         self.maxAcc = maxArray
00961 
00962 
00963     def setGyroData(self, gyroArray):
00964         self.gyroVal = gyroArray
00965         
00966     def setStdevGyro(self, stdevArray):
00967         self.stdevGyro = stdevArray
00968         
00969     def setMeanGyro(self, meanArray):
00970         self.meanGyro = meanArray
00971 
00972     def setMaxGyro(self, maxArray):
00973         self.maxGyro = maxArray
00974       
00975     def setGyroData(self, gyroVal):
00976       self.gyroVal = gyroVal
00977                  


wiimote
Author(s): Andreas Paepcke, Melonee Wise, Mark Horn
autogenerated on Sat Jun 8 2019 20:54:42