From ce669eb320905cb78abb2e9aa795d64549666890 Mon Sep 17 00:00:00 2001
From: Andrey Pohilko <apc4@ya.ru>
Date: Fri, 15 Sep 2017 21:15:51 +0300
Subject: [PATCH] vernie carton field demo

---
 pylgbst/peripherals.py |  3 ++
 vernie.py              | 76 ++++++++++++++++++++++++++----------------
 2 files changed, 50 insertions(+), 29 deletions(-)

diff --git a/pylgbst/peripherals.py b/pylgbst/peripherals.py
index cd003c1..c00a8c5 100644
--- a/pylgbst/peripherals.py
+++ b/pylgbst/peripherals.py
@@ -77,6 +77,9 @@ class LED(Peripheral):
     SOMETHING = b'\x51\x00'
 
     def set_color(self, color, do_notify=True):
+        if color == COLOR_NONE:
+            color = COLOR_BLACK
+
         if color not in COLORS:
             raise ValueError("Color %s is not in list of available colors" % color)
 
diff --git a/vernie.py b/vernie.py
index 1a390c5..6ec6b52 100644
--- a/vernie.py
+++ b/vernie.py
@@ -1,12 +1,18 @@
 from pylgbst import *
 
-right = RIGHT = 1
-left = LEFT = -1
+forward = FORWARD = right = RIGHT = 1
+backward = BACKWARD = left = LEFT = -1
 straight = STRAIGHT = 0
 
 
 class Vernie(MoveHub):
-    def __init__(self, conn=None):
+    def __init__(self):
+        try:
+            conn = DebugServerConnection()
+        except BaseException:
+            logging.debug("Failed to use debug server: %s", traceback.format_exc())
+        conn = BLEConnection().connect()
+
         super(Vernie, self).__init__(conn)
 
         while True:
@@ -29,51 +35,63 @@ class Vernie(MoveHub):
         log.info("Vernie is ready.")
 
     def _external_motor_data(self, data):
-        #log.debug("External motor position: %s", data)
+        log.debug("External motor position: %s", data)
         self._head_position = data
 
     def _color_distance_data(self, color, distance):
-        #log.debug("Color & Distance data: %s %s", COLORS[color], distance)
+        log.debug("Color & Distance data: %s %s", COLORS[color], distance)
         self._sensor_distance = distance
-        if self._color_detected != color:
-            self._color_detected = color
-            self.led.set_color(self._color_detected if self._color_detected != COLOR_NONE else COLOR_BLACK)
+        self._color_detected = color
+        if self._color_detected != COLOR_NONE:
+            self.led.set_color(self._color_detected)
 
     def _reset_head(self):
         self.motor_external.timed(1, -0.2)
-        self.head_to(RIGHT, angle=45)
+        self.head_to(RIGHT, speed=45)
 
-    def head_to(self, direction=RIGHT, speed=0.1, angle=25):
+    def head_to(self, direction=RIGHT, angle=25, speed=0.1):
         if direction == STRAIGHT:
             angle = -self._head_position
             direction = 1
 
         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.motor_AB.angled(225 * degrees / 90, speed * direction, -speed * direction)
+        self.head_to(STRAIGHT, speed=1)
+
+    def move(self, direction, distance=1, speed=0.3):
+        self.head_to(STRAIGHT, speed=0.5)
+        self.motor_AB.angled(distance * 450, speed * direction, speed * direction)
+
     def program(self):
-        while True:
-            self.head_to(LEFT)
-            time.sleep(1)
-
-            self.head_to(STRAIGHT)
-            time.sleep(1)
-
-            self.head_to(RIGHT)
-            time.sleep(1)
-
-            self.head_to(STRAIGHT)
-            time.sleep(1)
+        # while True:
+        self.move(FORWARD)
+        self.move(FORWARD)
+        self.turn(RIGHT)
+        self.move(FORWARD)
+        self.turn(LEFT)
+        self.move(FORWARD)
+        self.turn(RIGHT)
+        self.move(BACKWARD)
+        self.move(BACKWARD)
+        self.turn(LEFT)
+        self.move(FORWARD)
+        self.move(FORWARD)
+        self.turn(RIGHT)
+        self.move(FORWARD)
+        self.turn(RIGHT)
+        self.move(FORWARD, 3)
+        self.turn(LEFT)
+        self.turn(LEFT)
+        self.move(BACKWARD, 2)
 
 
 if __name__ == '__main__':
     logging.basicConfig(level=logging.DEBUG)
     comms.log.setLevel(logging.INFO)
 
-    try:
-        connection = DebugServerConnection()
-    except BaseException:
-        logging.warning("Failed to use debug server: %s", traceback.format_exc())
-        connection = BLEConnection().connect()
-
-    vernie = Vernie(connection)
+    vernie = Vernie()
     vernie.program()