From b590d13852202aba02c0ce08ab8cbccb4fd0924a Mon Sep 17 00:00:00 2001
From: LeonBondeLarsen <lelar09@student.sdu.dk>
Date: Sat, 9 Mar 2013 15:04:24 +0100
Subject: [PATCH] Modified wiimote to allow fixed MAC address

---
 wiimote/nodes/wiimote_node.py  |  8 +++++++-
 wiimote/src/wiimote/WIIMote.py | 20 ++++++++++++++------
 2 files changed, 21 insertions(+), 7 deletions(-)

diff --git a/wiimote/nodes/wiimote_node.py b/wiimote/nodes/wiimote_node.py
index a3fc7df0..b6891af5 100755
--- a/wiimote/nodes/wiimote_node.py
+++ b/wiimote/nodes/wiimote_node.py
@@ -105,7 +105,13 @@ def runWiimoteNode(self):
         # and are handled there:
         
         rospy.init_node('wiimote', anonymous=True, log_level=rospy.ERROR) # log_level=rospy.DEBUG
-        wiimoteDevice = wiimote.WIIMote.WIIMote()
+        self.addr = rospy.get_param("~fixed_wiimote_address","")
+        
+        if ":" in self.addr :
+            wiimoteDevice = wiimote.WIIMote.WIIMote(self.addr)
+        else :
+            wiimoteDevice = wiimote.WIIMote.WIIMote()
+            
         wiimoteDevice.zeroDevice()
         
         try:
diff --git a/wiimote/src/wiimote/WIIMote.py b/wiimote/src/wiimote/WIIMote.py
index c88ed6f3..1113815a 100644
--- a/wiimote/src/wiimote/WIIMote.py
+++ b/wiimote/src/wiimote/WIIMote.py
@@ -92,16 +92,15 @@ class WIIMote(object):
       
   
   """
-
+      
   # Public constants:
   
   BATTERY_MAX = cwiid.BATTERY_MAX  # 208 a.k.a. 0xD0
   
   # Public vars:
-
-  
+  _fixed_addr = False
+  _addr = ""
   # Private constants:
-
   
   # Private vars:
 
@@ -132,7 +131,7 @@ class WIIMote(object):
   # __init__
   #------------------
 
-  def __init__(self, theSampleRate=0, wiiStateLock=None, gatherCalibrationStats=False):
+  def __init__(self, addr="", theSampleRate=0, wiiStateLock=None, gatherCalibrationStats=False):
     """Instantiate a Wiimote driver instance, which controls one physical Wiimote device.
     
     Parameters:
@@ -142,6 +141,12 @@ def __init__(self, theSampleRate=0, wiiStateLock=None, gatherCalibrationStats=Fa
             theSampleRate=  x: every x seconds   
     """
 
+    # Use fixed MAC address for wiimote if available
+    self._addr = addr
+    if ":" in self._addr :
+      self._fixed_addr = True
+
+    
     self.lastZeroingTime = 0.
     
     self.gatherCalibrationStats = gatherCalibrationStats
@@ -189,7 +194,10 @@ def __init__(self, theSampleRate=0, wiiStateLock=None, gatherCalibrationStats=Fa
     promptUsr("Press buttons 1 and 2 together to pair (within 6 seconds).\n    (If no blinking lights, press power button for ~3 seconds.)")
 
     try:
-      self._wm = cwiid.Wiimote()
+      if self._fixed_addr :
+        self._wm = cwiid.Wiimote(self._addr)
+      else :
+        self._wm = cwiid.Wiimote()
     except RuntimeError:
       raise WiimoteNotFoundError("No Wiimote found to pair with.")
       exit()