From 3390e5243cf974eff6dd23a2abde3fd33dfe054e Mon Sep 17 00:00:00 2001
From: Andrey Pohilko <apc4@ya.ru>
Date: Wed, 13 Sep 2017 20:34:34 +0300
Subject: [PATCH] Handle port status notification

---
 demo.py                  | 17 ++++++++++++-----
 pylegoboost/__init__.py  | 26 ++++++++++++++++++++++++--
 pylegoboost/constants.py |  5 +++++
 test.py                  |  4 +++-
 4 files changed, 44 insertions(+), 8 deletions(-)

diff --git a/demo.py b/demo.py
index 668cbd2..6714114 100644
--- a/demo.py
+++ b/demo.py
@@ -28,7 +28,7 @@ def demo_motors_timed(movehub):
     log.info("Motors movement demo: timed")
     for level in range(0, 101, 5):
         level /= 100.0
-        log.info("Speed level: %s%%", level)
+        log.info("Speed level: %s%%", level * 100)
         movehub.motor_A.timed(0.2, level)
         movehub.motor_B.timed(0.2, -level)
     movehub.motor_AB.timed(1.5, -0.2, 0.2)
@@ -54,16 +54,23 @@ def demo_motors_angled(movehub):
 def demo_port_cd_motor(movehub):
     motor = None
     if isinstance(movehub.port_D, EncodedMotor):
+        log.info("Rotation motor is on port D")
         motor = movehub.port_D
     elif isinstance(movehub.port_C, EncodedMotor):
-        motor = movehub.port_D
+        log.info("Rotation motor is on port C")
+        motor = movehub.port_C
     else:
         log.warning("Motor not found on ports C or D")
 
     if motor:
-        motor.angled(20, 1)
+        motor.angled(20, 0.2)
+        sleep(3)
+        motor.angled(20, -0.2)
         sleep(1)
-        motor.angled(20, -1)
+
+        motor.angled(20, -0.1)
+        sleep(2)
+        motor.angled(20, 0.1)
         sleep(1)
 
 
@@ -94,6 +101,6 @@ if __name__ == '__main__':
     sleep(1)
     # hub.get_name()
     demo_port_cd_motor(hub)
-    # demo_all(hub)
+    #demo_all(hub)
     # demo_led_colors(hub)
     sleep(1)
diff --git a/pylegoboost/__init__.py b/pylegoboost/__init__.py
index 9411647..cf00861 100644
--- a/pylegoboost/__init__.py
+++ b/pylegoboost/__init__.py
@@ -57,11 +57,26 @@ class MoveHub(object):
 
         if msg_type == MSG_PORT_INFO:
             self._handle_port_info(data)
+        elif msg_type == MSG_PORT_STATUS:
+            self._handle_port_status(data)
         else:
-            log.warning("Unhandled msg type %s: %s", msg_type, orig.encode("hex"))
+            log.warning("Unhandled msg type 0x%x: %s", msg_type, orig.encode("hex"))
 
         pass
 
+    def _handle_port_status(self, data):
+        port = ord(data[3])
+        status = ord(data[4])
+
+        if status == STATUS_STARTED:
+            self.devices[port].started()
+        elif status == STATUS_FINISHED:
+            self.devices[port].finished()
+        elif status == STATUS_CONFLICT:
+            log.warning("Command conflict on port %s", PORTS[port])
+        else:
+            log.warning("Unhandled status value: 0x%x", status)
+
     def _handle_port_info(self, data):
         port = ord(data[3])
         dev_type = ord(data[5])
@@ -69,7 +84,7 @@ class MoveHub(object):
         if port in PORTS and dev_type in DEVICE_TYPES:
             log.debug("Device %s at port %s", DEVICE_TYPES[dev_type], PORTS[port])
         else:
-            log.debug("Device 0x%x at port 0x%x", dev_type, port)
+            log.warning("Device 0x%x at port 0x%x", dev_type, port)
 
         if dev_type == TYPE_MOTOR:
             self.devices[port] = EncodedMotor(self, port)
@@ -114,6 +129,7 @@ class Peripheral(object):
         super(Peripheral, self).__init__()
         self.parent = parent
         self.port = port
+        self.working = False
 
     def _set_port_val(self, value):
         cmd = self.PACKET_VER + self.SET_PORT_VAL + chr(self.port)
@@ -121,6 +137,12 @@ class Peripheral(object):
 
         self.parent.connection.write(MOVE_HUB_HARDWARE_HANDLE, chr(len(cmd)) + cmd)
 
+    def started(self):
+        self.working = True
+
+    def finished(self):
+        self.working = False
+
 
 class LED(Peripheral):
     def set_color(self, color):
diff --git a/pylegoboost/constants.py b/pylegoboost/constants.py
index a13c4b0..0010e5c 100644
--- a/pylegoboost/constants.py
+++ b/pylegoboost/constants.py
@@ -58,6 +58,7 @@ PORTS = {
 
 # NOTIFICATIONS
 MSG_PORT_INFO = 0x04
+MSG_PORT_STATUS = 0x82
 
 TYPE_DISTANCE_COLOR_SENSOR = 0x25
 TYPE_IMOTOR = 0x26
@@ -77,3 +78,7 @@ DEVICE_TYPES = {
     TYPE_SOMETHING1: "UNK1",
     TYPE_SOMETHING2: "UNK2",
 }
+
+STATUS_STARTED = 0x01
+STATUS_CONFLICT = 0x05
+STATUS_FINISHED = 0x0a
diff --git a/test.py b/test.py
index 18a880b..c49b66d 100644
--- a/test.py
+++ b/test.py
@@ -62,8 +62,10 @@ class GeneralTest(unittest.TestCase):
         conn.notifications.append((14, '1b0e00 0f00 04 3a 0128000000000100000001'))
         conn.notifications.append((14, '1b0e00 0f00 04 3b 0115000200000002000000'))
         conn.notifications.append((14, '1b0e00 0f00 04 3c 0114000200000002000000'))
+        conn.notifications.append((14, '1b0e00 0f00 8202 01'))
+        conn.notifications.append((14, '1b0e00 0f00 8202 0a'))
         time.sleep(1)
-        demo_all(hub)
+        #demo_all(hub)
         conn.running = False
 
         while not conn.finished: