From 3c82405040bf84636f010236821e51ae2058d883 Mon Sep 17 00:00:00 2001
From: Andrey Pohilko <apc4@ya.ru>
Date: Wed, 13 Sep 2017 13:24:53 +0300
Subject: [PATCH] Angled motor works

---
 demo.py                 | 15 +++++++++++++--
 pylegoboost/__init__.py | 32 ++++++++++++++++++++++----------
 pylegoboost/comms.py    |  2 +-
 3 files changed, 36 insertions(+), 13 deletions(-)

diff --git a/demo.py b/demo.py
index 284dc98..08f5cbd 100644
--- a/demo.py
+++ b/demo.py
@@ -11,7 +11,8 @@ log = logging.getLogger("demo")
 def demo_all(conn):
     movehub = MoveHub(conn)
     # demo_led_colors(movehub)
-    demo_motors_timed(movehub)
+    # demo_motors_timed(movehub)
+    demo_motors_angled(movehub)
 
 
 def demo_led_colors(movehub):
@@ -35,8 +36,18 @@ def demo_motors_timed(movehub):
     movehub.motor_AB.timed(0.5, -1)
 
 
+def demo_motors_angled(movehub):
+    log.info("Motors movement demo: angled")
+    for angle in range(0, 361, 90):
+        log.info("Angle: %s", angle)
+        movehub.motor_B.angled(angle, 1)
+        sleep(1)
+        movehub.motor_B.angled(angle, -1)
+        sleep(1)
+
+
 if __name__ == '__main__':
-    logging.basicConfig(level=logging.INFO)
+    logging.basicConfig(level=logging.DEBUG)
 
     try:
         connection = DebugServerConnection()
diff --git a/pylegoboost/__init__.py b/pylegoboost/__init__.py
index 8e2f007..eb187bc 100644
--- a/pylegoboost/__init__.py
+++ b/pylegoboost/__init__.py
@@ -1,8 +1,11 @@
+import logging
 import struct
 import time
 
 from pylegoboost.constants import *
 
+log = logging.getLogger('movehub')
+
 
 class MoveHub(object):
     """
@@ -48,13 +51,14 @@ class LED(Peripheral):
 
 
 class EncodedMotor(Peripheral):
+    TRAILER = b'\x64\x7f\x03'  # NOTE: \x64 is 100, might mean something
     PACKET_VER = b'\x01'
     SET_PORT_VAL = b'\x81'
-    MOTOR_TIMED_END = b'\x64\x7f\x03'
-    TRAILER = b'\x64\x7f\x03'  # NOTE: \x64 is 100, might mean something
-    TIMED_GROUP = b'\x0A'
-    TIMED_SINGLE = b'\x09'
     MOVEMENT_TYPE = b'\x11'
+    TIMED_SINGLE = b'\x09'
+    TIMED_GROUP = b'\x0A'
+    ANGLED_SINGLE = b'\x0B'
+    ANGLED_GROUP = b'\x0C'
 
     def __init__(self, parent, port):
         super(EncodedMotor, self).__init__(parent)
@@ -66,10 +70,10 @@ class EncodedMotor(Peripheral):
         if relative < -1 or relative > 1:
             raise ValueError("Invalid speed value: %s", relative)
 
-        relative *= 255
-        if relative < 0:
-            relative += 255
-        return int(relative)
+        absolute = round(relative * 100)
+        if absolute < 0:
+            absolute += 255
+        return int(absolute)
 
     def _wrap_and_write(self, command, speed_primary, speed_secondary):
         # set for port
@@ -97,8 +101,16 @@ class EncodedMotor(Peripheral):
         if not async:
             time.sleep(seconds)
 
-    def angled(self, angle, speed_primary, speed_secondary):
-        pass
+    def angled(self, angle, speed_primary=1, speed_secondary=None):
+        if speed_secondary is None:
+            speed_secondary = speed_primary
+
+        # movement type
+        command = self.ANGLED_GROUP if self.port == PORT_AB else self.ANGLED_SINGLE
+        # angle
+        command += struct.pack('<I', angle)
+
+        self._wrap_and_write(command, speed_primary, speed_secondary)
 
 
 class ColorDistanceSensor(Peripheral):
diff --git a/pylegoboost/comms.py b/pylegoboost/comms.py
index 7aa7e66..80bf274 100644
--- a/pylegoboost/comms.py
+++ b/pylegoboost/comms.py
@@ -159,7 +159,7 @@ class DebugServer(object):
                 buf = buf[buf.index("\n") + 1:]
 
                 if line:
-                    log.debug("Cmd line: %s", line)
+                    log.info("Cmd line: %s", line)
                     try:
                         self._handle_cmd(json.loads(line))
                     except BaseException: