$search
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