From afe5c3a55a6633f0f2728bb9596a852648fe4c39 Mon Sep 17 00:00:00 2001
From: Andrey Pohilko <apc4@ya.ru>
Date: Wed, 13 Sep 2017 13:34:21 +0300
Subject: [PATCH] Tested motor on port D

---
 demo.py                 | 16 ++++++++++++++--
 pylegoboost/__init__.py | 10 +++++++---
 2 files changed, 21 insertions(+), 5 deletions(-)

diff --git a/demo.py b/demo.py
index 08f5cbd..10936ec 100644
--- a/demo.py
+++ b/demo.py
@@ -2,7 +2,7 @@ import logging
 import traceback
 from time import sleep
 
-from pylegoboost import MoveHub, COLORS
+from pylegoboost import MoveHub, COLORS, EncodedMotor, PORT_D
 from pylegoboost.comms import DebugServerConnection, BLEConnection
 
 log = logging.getLogger("demo")
@@ -12,7 +12,8 @@ def demo_all(conn):
     movehub = MoveHub(conn)
     # demo_led_colors(movehub)
     # demo_motors_timed(movehub)
-    demo_motors_angled(movehub)
+    # demo_motors_angled(movehub)
+    demo_port_c_motor(movehub)
 
 
 def demo_led_colors(movehub):
@@ -45,6 +46,17 @@ def demo_motors_angled(movehub):
         movehub.motor_B.angled(angle, -1)
         sleep(1)
 
+    movehub.motor_AB.angled(360, 1, -1)
+    sleep(1)
+    movehub.motor_AB.angled(360, -1, 1)
+
+
+def demo_port_c_motor(movehub):
+    portd = EncodedMotor(movehub, PORT_D)
+    portd.angled(90, 1)
+    sleep(1)
+    portd.angled(90, -1)
+
 
 if __name__ == '__main__':
     logging.basicConfig(level=logging.DEBUG)
diff --git a/pylegoboost/__init__.py b/pylegoboost/__init__.py
index eb187bc..86b44d1 100644
--- a/pylegoboost/__init__.py
+++ b/pylegoboost/__init__.py
@@ -21,8 +21,8 @@ class MoveHub(object):
         self.motor_B = EncodedMotor(self, PORT_B)
         self.motor_AB = EncodedMotor(self, PORT_AB)
 
-        # self.port_c
-        # self.port_d
+        self.port_c = None
+        self.port_d = None
 
         # self.button
         # self.tilt_sensor
@@ -94,7 +94,10 @@ class EncodedMotor(Peripheral):
         # movement type
         command = self.TIMED_GROUP if self.port == PORT_AB else self.TIMED_SINGLE
         # time
-        command += struct.pack('<H', int(seconds * 1000))
+        msec = int(seconds * 1000)
+        if msec > 255 * 255:
+            raise ValueError("Too large value for seconds: %s", seconds)
+        command += struct.pack('<H', msec)
 
         self._wrap_and_write(command, speed_primary, speed_secondary)
 
@@ -111,6 +114,7 @@ class EncodedMotor(Peripheral):
         command += struct.pack('<I', angle)
 
         self._wrap_and_write(command, speed_primary, speed_secondary)
+        # TODO: how to tell when motor has stopped?
 
 
 class ColorDistanceSensor(Peripheral):