From d52581c2363b13eb76c9ca2247bef2a90a37cf5c Mon Sep 17 00:00:00 2001
From: Andrey Pohilko <apc4@ya.ru>
Date: Fri, 15 Sep 2017 22:26:33 +0300
Subject: [PATCH] found where battery voltage is

---
 demo.py                | 13 +++++++++----
 pylgbst/__init__.py    |  6 ++++++
 pylgbst/constants.py   |  9 ++++-----
 pylgbst/peripherals.py | 16 +++++++---------
 vernie.py              | 12 ++++++------
 5 files changed, 32 insertions(+), 24 deletions(-)

diff --git a/demo.py b/demo.py
index 9b7ab28..43804e8 100644
--- a/demo.py
+++ b/demo.py
@@ -157,6 +157,10 @@ def demo_all(movehub):
     demo_motor_sensors(movehub)
 
 
+def cb_log(val1, val2=None, val3=None):
+    log.info("V1:%s\tV2:%s\tV3:%s", unpack("<H", val1), val2, val3)
+
+
 if __name__ == '__main__':
     logging.basicConfig(level=logging.INFO)
 
@@ -168,8 +172,9 @@ if __name__ == '__main__':
 
     hub = MoveHub(connection)
 
-    demo_motors_timed(hub)
-    # demo_all(hub)
-
-    log.info("Sleeping 60s")
+    hub.devices[PORT_BATTERY].subscribe(cb_log, 0x00, granularity=2)
+    sleep(10)
+    demo_port_cd_motor(hub)
     sleep(60)
+    hub.devices[PORT_BATTERY].unsubscribe(cb_log)
+    # demo_all(hub)
diff --git a/pylgbst/__init__.py b/pylgbst/__init__.py
index 0916271..ab002a0 100644
--- a/pylgbst/__init__.py
+++ b/pylgbst/__init__.py
@@ -12,6 +12,7 @@ class MoveHub(object):
     :type led: LED
     :type tilt_sensor: TiltSensor
     :type button: Button
+    :type battery: Battery
     :type color_distance_sensor: pylgbst.peripherals.ColorDistanceSensor
     :type motor_external: EncodedMotor
     :type port_C: Peripheral
@@ -31,6 +32,7 @@ class MoveHub(object):
         # shorthand fields
         self.button = Button(self)
         self.led = None
+        self.battery = None
         self.motor_A = None
         self.motor_B = None
         self.motor_AB = None
@@ -133,6 +135,8 @@ class MoveHub(object):
             self.devices[port] = LED(self, port)
         elif dev_type == DEV_TILT_SENSOR:
             self.devices[port] = TiltSensor(self, port)
+        elif dev_type == DEV_BATTERY:
+            self.devices[port] = Battery(self, port)
         else:
             log.warning("Unhandled peripheral type 0x%x on port 0x%x", dev_type, port)
             self.devices[port] = Peripheral(self, port)
@@ -151,5 +155,7 @@ class MoveHub(object):
             self.led = self.devices[port]
         elif port == PORT_TILT_SENSOR:
             self.tilt_sensor = self.devices[port]
+        elif port == PORT_BATTERY:
+            self.battery = self.devices[port]
         else:
             log.warning("Unhandled port: %s", PORTS[port])
diff --git a/pylgbst/constants.py b/pylgbst/constants.py
index 2f48c38..aab86ae 100644
--- a/pylgbst/constants.py
+++ b/pylgbst/constants.py
@@ -18,7 +18,7 @@ PORT_B = 0x38
 PORT_AB = 0x39
 PORT_TILT_SENSOR = 0x3a
 PORT_SOMETHING1 = 0x3B
-PORT_SOMETHING2 = 0x3C
+PORT_BATTERY = 0x3C  # subscribing on this port showed correlation with power voltage
 
 PORTS = {
     PORT_A: "A",
@@ -29,7 +29,7 @@ PORTS = {
     PORT_LED: "LED",
     PORT_TILT_SENSOR: "TILT_SENSOR",
     PORT_SOMETHING1: "UNK1",
-    PORT_SOMETHING2: "UNK2",
+    PORT_BATTERY: "BATTERY",
 }
 
 # PACKET TYPES
@@ -47,8 +47,7 @@ MSG_SENSOR_SUBSCRIBE_ACK = 0x47
 
 # DEVICE TYPES
 DEV_UNKNOWN1 = 0x15  # one of them is button?
-DEV_UNKNOWN2 = 0x14  # another is battery?
-
+DEV_BATTERY = 0x14
 DEV_DCS = 0x25
 DEV_IMOTOR = 0x26
 DEV_MOTOR = 0x27
@@ -62,7 +61,7 @@ DEVICE_TYPES = {
     DEV_TILT_SENSOR: "TILT_SENSOR",
     DEV_LED: "LED",
     DEV_UNKNOWN1: "UNKNOWN #1",
-    DEV_UNKNOWN2: "UNKNOWN #2",
+    DEV_BATTERY: "UNKNOWN #2",
 }
 
 # NOTIFICATIONS
diff --git a/pylgbst/peripherals.py b/pylgbst/peripherals.py
index c00a8c5..7957379 100644
--- a/pylgbst/peripherals.py
+++ b/pylgbst/peripherals.py
@@ -70,7 +70,8 @@ class Peripheral(object):
             subscriber(*args, **kwargs)
 
     def handle_port_data(self, data):
-        log.warning("Unhandled device notification for %s: %s", self, str2hex(data))
+        log.warning("Unhandled device notification for %s: %s", self, str2hex(data[4:]))
+        self._notify_subscribers(data[4:])
 
 
 class LED(Peripheral):
@@ -174,8 +175,6 @@ class EncodedMotor(Peripheral):
                 time.sleep(0.5)
             log.debug("Command has finished.")
 
-    # TODO: how to tell when motor has stopped?
-
     def handle_port_data(self, data):
         if self._port_subscription_mode == MOTOR_MODE_ANGLE:
             rotation = unpack("<l", data[4:8])[0]
@@ -273,12 +272,11 @@ class ColorDistanceSensor(Peripheral):
             log.debug("Unhandled data in mode %s: %s", self._port_subscription_mode, str2hex(data))
 
 
+class Battery(Peripheral):
+    def handle_port_data(self, data):
+        self._notify_subscribers(unpack("<H", data))
+
+
 class Button(Peripheral):
     def __init__(self, parent):
         super(Button, self).__init__(parent, 0)
-
-
-LISTEN_ENCODER_ON_A = b'        \x0a\x00 \x41\x37 \x02\x01\x00\x00\x00\x01'
-LISTEN_ENCODER_ON_B = b'        \x0a\x00 \x41\x38 \x02\x01\x00\x00\x00\x01'
-LISTEN_ENCODER_ON_C = b'        \x0a\x00 \x41\x01 \x02\x01\x00\x00\x00\x01'
-LISTEN_ENCODER_ON_D = b'        \x0a\x00 \x41\x02 \x02\x01\x00\x00\x00\x01'
diff --git a/vernie.py b/vernie.py
index 6ec6b52..1d74798 100644
--- a/vernie.py
+++ b/vernie.py
@@ -47,9 +47,9 @@ class Vernie(MoveHub):
 
     def _reset_head(self):
         self.motor_external.timed(1, -0.2)
-        self.head_to(RIGHT, speed=45)
+        self.head(RIGHT, speed=45)
 
-    def head_to(self, direction=RIGHT, angle=25, speed=0.1):
+    def head(self, direction=RIGHT, angle=25, speed=0.1):
         if direction == STRAIGHT:
             angle = -self._head_position
             direction = 1
@@ -57,13 +57,13 @@ class Vernie(MoveHub):
         self.motor_external.angled(direction * angle, speed)
 
     def turn(self, direction, degrees=90, speed=0.3):
-        self.head_to(STRAIGHT, speed=1)
-        self.head_to(direction, 35, 1)
+        self.head(STRAIGHT, speed=1)
+        self.head(direction, 35, 1)
         self.motor_AB.angled(225 * degrees / 90, speed * direction, -speed * direction)
-        self.head_to(STRAIGHT, speed=1)
+        self.head(STRAIGHT, speed=1)
 
     def move(self, direction, distance=1, speed=0.3):
-        self.head_to(STRAIGHT, speed=0.5)
+        self.head(STRAIGHT, speed=0.5)
         self.motor_AB.angled(distance * 450, speed * direction, speed * direction)
 
     def program(self):