From c255a38b437d67525044bf40f92a8728b2f50c16 Mon Sep 17 00:00:00 2001
From: Andrey Pohilko <apc4@ya.ru>
Date: Wed, 20 Sep 2017 19:34:42 +0300
Subject: [PATCH] Work on vernie demo polish

---
 README.md                |  4 ++--
 pylgbst/peripherals.py   | 16 ++++++++--------
 vernie/__init__.py       |  2 +-
 vernie/android_remote.py | 26 ++++++++++++++++++++------
 4 files changed, 31 insertions(+), 17 deletions(-)

diff --git a/README.md b/README.md
index f313a7b..241b997 100644
--- a/README.md
+++ b/README.md
@@ -50,11 +50,11 @@ Methods to activate motors are:
 - `constant(speed_primary, speed_secondary)` - enables motor with specified speed forever 
 - `timed(time, speed_primary, speed_secondary)` - enables motor with specified speed for `time` seconds, float values accepted
 - `angled(angle, speed_primary, speed_secondary)` - makes motor to rotate to specified angle, `angle` value is integer degrees, can be negative and can be more than 360 for several rounds
-- `stop()` - stops motor at once, it is equivalent for `constant(0, async=True)`
+- `stop()` - stops motor at once, it is equivalent for `constant(0)`
 
 Parameter `speed_secondary` is used when it is motor group of `motor_AB` running together. By default, `speed_secondary` equals `speed_primary`.
 
-All these methods are synchronous by default, except `stop()`. You can pass `async` parameter to any of them to switch into asynchronous, which means command will return immediately, without waiting for rotation to complete.
+All these methods are synchronous by default, means method does not return untill it gets confirmation from Hub that command has completed. You can pass `async=True` parameter to any of methods to switch into asynchronous, which means command will return immediately, without waiting for rotation to complete. Be careful with asynchronous calls, as they make Hub to stop reporting synchronizing statuses.
 
 An example:
 ```python
diff --git a/pylgbst/peripherals.py b/pylgbst/peripherals.py
index 0ac341d..26b010d 100644
--- a/pylgbst/peripherals.py
+++ b/pylgbst/peripherals.py
@@ -228,7 +228,7 @@ class EncodedMotor(Peripheral):
         self._wrap_and_write(self.SOME_SINGLE, b"", speed_primary, speed_secondary)
         self._wait_sync(async)
 
-    def stop(self, async=True):
+    def stop(self, async=False):
         self.constant(0, async=async)
 
     def handle_port_data(self, data):
@@ -329,23 +329,23 @@ class ColorDistanceSensor(Peripheral):
 
     def handle_port_data(self, data):
         if self._port_subscription_mode == self.COLOR_DISTANCE_FLOAT:
-            color = data[4]
-            distance = data[5]
-            partial = data[7]
+            color = unpack("<B", data[4:5])[0]
+            distance = unpack("<B", data[6:7])[0]
+            partial = unpack("<B", data[7:0])[0]
             if partial:
                 distance += 1.0 / partial
             self._notify_subscribers(color, float(distance))
         elif self._port_subscription_mode == self.COLOR_ONLY:
-            color = data[4]
+            color = unpack("<B", data[4:5])[0]
             self._notify_subscribers(color)
         elif self._port_subscription_mode == self.DISTANCE_INCHES:
-            distance = data[4]
+            distance = unpack("<B", data[4:5])[0]
             self._notify_subscribers(distance)
         elif self._port_subscription_mode == self.DISTANCE_HOW_CLOSE:
-            distance = data[4]
+            distance = unpack("<B", data[4:5])[0]
             self._notify_subscribers(distance)
         elif self._port_subscription_mode == self.DISTANCE_SUBINCH_HOW_CLOSE:
-            distance = data[4]
+            distance = unpack("<B", data[4:5])[0]
             self._notify_subscribers(distance)
         elif self._port_subscription_mode == self.OFF1 or self._port_subscription_mode == self.OFF2:
             log.info("Turned off led on %s", self)
diff --git a/vernie/__init__.py b/vernie/__init__.py
index d3da99d..e04373c 100644
--- a/vernie/__init__.py
+++ b/vernie/__init__.py
@@ -80,7 +80,7 @@ class Vernie(MoveHub):
         self.motor_external.subscribe(self._external_motor_data)
 
         self._reset_head()
-        # self.say("ready")
+        self.say("ready")
         time.sleep(1)
 
     def say(self, phrase):
diff --git a/vernie/android_remote.py b/vernie/android_remote.py
index 3c83968..a3d4198 100644
--- a/vernie/android_remote.py
+++ b/vernie/android_remote.py
@@ -9,6 +9,7 @@ import logging
 import socket
 import time
 
+from pylgbst.peripherals import ColorDistanceSensor
 from vernie import Vernie
 
 host = ''
@@ -34,6 +35,15 @@ def ranged(param):
     return float(param / 10)
 
 
+front_distance = 0
+
+
+def on_distance(distance):
+    logging.info("Distance %s", distance)
+    global front_distance
+    front_distance = distance
+
+
 udp_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
 udp_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
 udp_sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
@@ -44,8 +54,7 @@ robot = Vernie()
 robot.button.subscribe(on_btn)
 robot.motor_AB.stop()
 
-# TODO: use distance sensor to stop movement forward
-
+robot.color_distance_sensor.subscribe(on_distance, ColorDistanceSensor.DISTANCE_INCHES)
 try:
     udp_sock.bind((host, port))
     time.sleep(1)
@@ -68,11 +77,16 @@ try:
         messageString = message.decode("utf-8")
         a, b, c = decode_xml(messageString)
         divider = 2.0 if c > 0 else -2.0
-        sa = round(c + b / divider, 2)
-        sb = round(c - b / divider, 2)
+
+        if 0 < front_distance < 9 and c > 0:
+            logging.info("Something in front of Vernie [%s]!", front_distance)
+            c = 0
+
+        sa = round(c + b / divider, 1)
+        sb = round(c - b / divider, 1)
         logging.info("SpeedA=%s, SpeedB=%s", sa, sb)
-        robot.motor_AB.constant(sa, sb, async=True)
-        time.sleep(0.5)
+        robot.motor_AB.constant(sa, sb)
+        # time.sleep(0.5)
 finally:
     robot.motor_AB.stop()
     udp_sock.close()