From f3a5aab7ef511e14436aad1f85e92f63f0f86598 Mon Sep 17 00:00:00 2001
From: Andrey Pohilko <apc4@ya.ru>
Date: Mon, 18 Sep 2017 18:50:56 +0300
Subject: [PATCH] Revealed constant motor mode

---
 README.md              |  3 ++-
 demo.py                | 14 ++++++++++++--
 pylgbst/peripherals.py | 41 +++++++++++++++++++++++++++++++++++++++++
 3 files changed, 55 insertions(+), 3 deletions(-)

diff --git a/README.md b/README.md
index 430c3c3..a26ab74 100644
--- a/README.md
+++ b/README.md
@@ -60,7 +60,7 @@ sudo python -c "from pylgbst.comms import *; \
     DebugServer(BLEConnection().connect()).start()"
 ```
 
-## Roadmap
+## TODO
 
 - handle device detach and device attach events on ports C/D
 - experiment with motor commands, find what is hidden there
@@ -71,6 +71,7 @@ sudo python -c "from pylgbst.comms import *; \
 - subscribing to 2 sensors at once causes port status to not arrive => sync mode stuck. Why?
 - can we subscribe to LED?
 - organize requesting and printing device info on startup - firmware version at least
+- make debug server to re-establish BLE connection on loss
 
 ## Links
 
diff --git a/demo.py b/demo.py
index c847232..71936bd 100644
--- a/demo.py
+++ b/demo.py
@@ -160,7 +160,7 @@ def demo_all(movehub):
 
 
 if __name__ == '__main__':
-    logging.basicConfig(level=logging.DEBUG)
+    logging.basicConfig(level=logging.INFO)
 
     try:
         connection = DebugServerConnection()
@@ -169,4 +169,14 @@ if __name__ == '__main__':
         connection = BLEConnection().connect()
 
     hub = MoveHub(connection)
-    demo_led_colors(hub)
+
+    mtr = hub.motor_A
+
+    # mtr.timed(5, async=True)
+    # time.sleep(1)
+    # mtr.stop()
+
+
+    mtr.constant(0.3, -0.5)
+    time.sleep(3)
+    mtr.stop()
diff --git a/pylgbst/peripherals.py b/pylgbst/peripherals.py
index c2ac3fe..5655be5 100644
--- a/pylgbst/peripherals.py
+++ b/pylgbst/peripherals.py
@@ -131,6 +131,9 @@ class LED(Peripheral):
 class EncodedMotor(Peripheral):
     TRAILER = b'\x64\x7f\x03'  # NOTE: \x64 is 100, might mean something
     MOVEMENT_TYPE = b'\x11'
+
+    CONSTANT_SINGLE = b'\x01'
+    CONSTANT_GROUP = b'\x02'
     TIMED_SINGLE = b'\x09'
     TIMED_GROUP = b'\x0A'
     ANGLED_SINGLE = b'\x0B'
@@ -193,6 +196,44 @@ class EncodedMotor(Peripheral):
         self._wrap_and_write(command, speed_primary, speed_secondary)
         self.__wait_sync(async)
 
+    def constant(self, speed_primary=1, speed_secondary=None, async=False):
+        if speed_secondary is None:
+            speed_secondary = speed_primary
+
+        # movement type
+        command = self.CONSTANT_GROUP if self.port == PORT_AB else self.CONSTANT_SINGLE
+
+        self.started()
+        self._wrap_and_write(command, speed_primary, speed_secondary)
+        self.__wait_sync(async)
+
+    def stop(self):
+        self.constant(0)
+
+    def test(self, speed_primary=1, speed_secondary=None):
+        if speed_secondary is None:
+            speed_secondary = speed_primary
+
+        self.started()
+
+        # self._wrap_and_write(command, 0.2, 0.2)
+
+        # set for port
+        command = self.MOVEMENT_TYPE + b"\x02"
+
+        command += pack('<B', self._speed_abs(speed_primary))  # time or angle - first param
+        command += pack('<B', self._speed_abs(speed_secondary))  # time or angle - first param
+
+        speed_primary = 0.5
+        speed_secondary = -0.5
+        # command += pack("<B", self._speed_abs(speed_primary))
+        # if self.port == PORT_AB:
+        #    command += pack("<B", self._speed_abs(speed_secondary))
+
+        # command += self.TRAILER
+
+        self._write_to_hub(MSG_SET_PORT_VAL, command)
+
     def __wait_sync(self, async):
         if not async:
             log.debug("Waiting for sync command work to finish...")