From 2536ed124973ed3c9d42e5b1cde589db2ab4b5bd Mon Sep 17 00:00:00 2001
From: Andrey Pohilko <apc4@ya.ru>
Date: Sun, 17 Sep 2017 22:03:38 +0300
Subject: [PATCH] Experimenting...

---
 README.md               |  8 +++++++-
 demo.py                 |  3 ++-
 pylgbst/peripherals.py  | 18 ++++++++++++------
 vernie/run_away_game.py | 11 ++++++-----
 4 files changed, 27 insertions(+), 13 deletions(-)

diff --git a/README.md b/README.md
index a810650..10fde67 100644
--- a/README.md
+++ b/README.md
@@ -38,6 +38,7 @@ for device in hub.devices:
 TODO: more usage instructions
 
 ### General Information
+connection params
 hub's devices detect process & fields to access them
 general subscription modes & granularity info
 
@@ -45,6 +46,7 @@ general subscription modes & granularity info
 ### Motor Rotation Sensors
 ### Tilt Sensor
 ### Color & Distance Sensor
+Tip: laser pointer pointing to sensor makes it trigger distance sensor
 ### LED
 ### Push Button
 ### Power Voltage & Battery
@@ -53,7 +55,9 @@ general subscription modes & granularity info
 ## Debug Server
 
 ```
-sudo python -c "from pylgbst.comms import *; import logging; logging.basicConfig(level=logging.DEBUG); DebugServer(BLEConnection().connect()).start()"
+sudo python -c "from pylgbst.comms import *; \
+    import logging; logging.basicConfig(level=logging.DEBUG); \
+    DebugServer(BLEConnection().connect()).start()"
 ```
 
 ## Roadmap
@@ -64,6 +68,8 @@ sudo python -c "from pylgbst.comms import *; import logging; logging.basicConfig
 - document all API methods
 - make sure unit tests cover all important code
 - generalize getting device info + give constants (low priority)
+- subscribing to color sensor disables LED operating in `run_away_game.py`. Why?
+- can we subscribe to LED?
 
 ## Links
 
diff --git a/demo.py b/demo.py
index f7b4a8d..c847232 100644
--- a/demo.py
+++ b/demo.py
@@ -9,6 +9,7 @@ log = logging.getLogger("demo")
 def demo_led_colors(movehub):
     # LED colors demo
     log.info("LED colors demo")
+    movehub.color_distance_sensor.subscribe(lambda x, y: None)
     for color in COLORS.keys()[1:] + [COLOR_BLACK]:
         log.info("Setting LED color to: %s", COLORS[color])
         movehub.led.set_color(color)
@@ -168,4 +169,4 @@ if __name__ == '__main__':
         connection = BLEConnection().connect()
 
     hub = MoveHub(connection)
-    demo_all(hub)
+    demo_led_colors(hub)
diff --git a/pylgbst/peripherals.py b/pylgbst/peripherals.py
index 9f3522d..c2ac3fe 100644
--- a/pylgbst/peripherals.py
+++ b/pylgbst/peripherals.py
@@ -57,7 +57,7 @@ class Peripheral(object):
         log.debug("Finished: %s", self)
         self._working = False
 
-    def is_working(self):
+    def in_progress(self):
         return bool(self._working)
 
     def subscribe(self, callback, mode, granularity=1):
@@ -99,6 +99,10 @@ class Peripheral(object):
 class LED(Peripheral):
     SOMETHING = b'\x51\x00'
 
+    def __init__(self, parent, port):
+        super(LED, self).__init__(parent, port)
+        self._last_color_set = COLOR_NONE
+
     def set_color(self, color, do_notify=True):
         if color == COLOR_NONE:
             color = COLOR_BLACK
@@ -106,20 +110,22 @@ class LED(Peripheral):
         if color not in COLORS:
             raise ValueError("Color %s is not in list of available colors" % color)
 
-        cmd = pack("<?", do_notify) + self.SOMETHING + pack("<B", color)
+        self._last_color_set = color
+        cmd = pack("<B", do_notify) + self.SOMETHING + pack("<B", color)
         self.started()
         self._write_to_hub(MSG_SET_PORT_VAL, cmd)
 
     def finished(self):
         super(LED, self).finished()
-        log.debug("LED has changed color")
-        self._notify_subscribers()
+        log.debug("LED has changed color to %s", COLORS[self._last_color_set])
+        self._notify_subscribers(self._last_color_set)
 
     def subscribe(self, callback, mode=None, granularity=None):
         self._subscribers.add(callback)
 
     def unsubscribe(self, callback=None):
-        self._subscribers.add(callback)
+        if callback in self._subscribers:
+            self._subscribers.remove(callback)
 
 
 class EncodedMotor(Peripheral):
@@ -190,7 +196,7 @@ class EncodedMotor(Peripheral):
     def __wait_sync(self, async):
         if not async:
             log.debug("Waiting for sync command work to finish...")
-            while self.is_working():
+            while self.in_progress():
                 time.sleep(0.5)
             log.debug("Command has finished.")
 
diff --git a/vernie/run_away_game.py b/vernie/run_away_game.py
index 336f359..3f5df2a 100644
--- a/vernie/run_away_game.py
+++ b/vernie/run_away_game.py
@@ -3,7 +3,6 @@ from vernie import *
 robot = Vernie()
 running = True
 
-robot.say("Place your hand in front of sensor")
 
 
 def callback(color, distance):
@@ -12,7 +11,7 @@ def callback(color, distance):
     print("Distance is %.1f inches, I'm running back with %s%% speed!" % (distance, int(speed * 100)))
     if speed <= 1:
         robot.motor_AB.timed(secs / 1, -speed, async=True)
-        robot.say("Place your hand in front of sensor")
+        robot.say("Ouch")
 
 
 def on_btn(pressed):
@@ -21,9 +20,10 @@ def on_btn(pressed):
         running = False
 
 
-robot.button.subscribe(on_btn)
-robot.color_distance_sensor.subscribe(callback)
 robot.led.set_color(COLOR_GREEN)
+robot.button.subscribe(on_btn)
+robot.say("Place your hand in front of sensor")
+robot.color_distance_sensor.subscribe(callback)
 
 while running:
     time.sleep(1)
@@ -31,4 +31,5 @@ while running:
 robot.color_distance_sensor.unsubscribe(callback)
 robot.button.unsubscribe(on_btn)
 robot.led.set_color(COLOR_NONE)
-time.sleep(10)  # let color change
+while robot.led.in_progress():
+    time.sleep(1)