From a6047b32793a865fb61b54b961a2dc5fb299599b Mon Sep 17 00:00:00 2001
From: Andrey Pohilko <apc4@ya.ru>
Date: Thu, 21 Sep 2017 13:42:20 +0300
Subject: [PATCH] Fixing vernie

---
 pylgbst/peripherals.py                        | 12 ++++++-----
 vernie/__init__.py                            | 21 +++++++++----------
 .../{carton_board_path.py => playmat_path.py} |  0
 vernie/run_away_game.py                       |  4 ++++
 4 files changed, 21 insertions(+), 16 deletions(-)
 rename vernie/{carton_board_path.py => playmat_path.py} (100%)

diff --git a/pylgbst/peripherals.py b/pylgbst/peripherals.py
index 26b010d..f0e861f 100644
--- a/pylgbst/peripherals.py
+++ b/pylgbst/peripherals.py
@@ -1,9 +1,9 @@
 import logging
 import time
+import traceback
 from struct import pack, unpack
 from threading import Thread
 
-# noinspection PyUnresolvedReferences
 from six.moves import queue
 
 from pylgbst.comms import str2hex
@@ -66,11 +66,12 @@ class Peripheral(object):
         self._port_subscription_mode = mode
         self.started()
         self._port_subscribe(self._port_subscription_mode, granularity, True)
-        if callback:
-            self._subscribers.add(callback)
 
         self._wait_sync(async)  # having async=True leads to stuck notifications
 
+        if callback:
+            self._subscribers.add(callback)
+
     def unsubscribe(self, callback=None):
         if callback in self._subscribers:
             self._subscribers.remove(callback)
@@ -98,6 +99,7 @@ class Peripheral(object):
             try:
                 self.handle_port_data(data)
             except BaseException:
+                log.warning("%s", traceback.format_exc())
                 log.warning("Failed to handle port data by %s: %s", self, str2hex(data))
 
     def _wait_sync(self, async):
@@ -330,8 +332,8 @@ class ColorDistanceSensor(Peripheral):
     def handle_port_data(self, data):
         if self._port_subscription_mode == self.COLOR_DISTANCE_FLOAT:
             color = unpack("<B", data[4:5])[0]
-            distance = unpack("<B", data[6:7])[0]
-            partial = unpack("<B", data[7:0])[0]
+            distance = unpack("<B", data[5:6])[0]
+            partial = unpack("<B", data[7:8])[0]
             if partial:
                 distance += 1.0 / partial
             self._notify_subscribers(color, float(distance))
diff --git a/vernie/__init__.py b/vernie/__init__.py
index e04373c..bd944ec 100644
--- a/vernie/__init__.py
+++ b/vernie/__init__.py
@@ -14,6 +14,7 @@ try:
 
 
     def say(text):
+        return
         if isinstance(text, str):
             text = text.decode("utf-8")
         md5 = hashlib.md5(text.encode('utf-8')).hexdigest()
@@ -55,12 +56,12 @@ SPEECH_LANG_MAP = {
     }
 }
 
-VERNIE_TO_MOTOR_DEGREES = 2.7
+VERNIE_TO_MOTOR_DEGREES = 2.6
+VERNIE_SINGLE_MOVE = 430
 
 
 class Vernie(MoveHub):
     def __init__(self, language='en'):
-        self.language = language
         try:
             conn = DebugServerConnection()
         except BaseException:
@@ -68,6 +69,7 @@ class Vernie(MoveHub):
             conn = BLEConnection().connect()
 
         super(Vernie, self).__init__(conn)
+        self.language = language
 
         while True:
             required_devices = (self.color_distance_sensor, self.motor_external)
@@ -104,14 +106,14 @@ class Vernie(MoveHub):
         self.motor_external.angled(direction * angle, speed)
 
     def turn(self, direction, degrees=90, speed=0.3):
-        self.head(STRAIGHT, speed=1)
-        self.head(direction, 35, 1)
+        #self.head(STRAIGHT, speed=0.5)
+        #self.head(direction, 35, 1)
         self.motor_AB.angled(int(VERNIE_TO_MOTOR_DEGREES * degrees), speed * direction, -speed * direction)
-        self.head(STRAIGHT, speed=1)
+        #self.head(STRAIGHT, speed=0.5)
 
-    def move(self, direction, distance=1, speed=0.3):
-        self.head(STRAIGHT, speed=0.5)
-        self.motor_AB.angled(distance * 450, speed * direction, speed * direction)
+    def move(self, direction, distance=1, speed=0.2):
+        #self.head(STRAIGHT, speed=0.5)
+        self.motor_AB.angled(distance * VERNIE_SINGLE_MOVE, speed * direction, speed * direction)
 
     def interpret_command(self, cmd, confirm):
         cmd = cmd.strip().lower().split(' ')
@@ -157,6 +159,3 @@ class Vernie(MoveHub):
         else:
             self.say("Unknown command")
             self.say("commands help")
-
-# TODO: disable motors if state is not up
-# TODO: stop motors on bump?
diff --git a/vernie/carton_board_path.py b/vernie/playmat_path.py
similarity index 100%
rename from vernie/carton_board_path.py
rename to vernie/playmat_path.py
diff --git a/vernie/run_away_game.py b/vernie/run_away_game.py
index 28bdbc3..49cb1a3 100644
--- a/vernie/run_away_game.py
+++ b/vernie/run_away_game.py
@@ -1,5 +1,9 @@
+import logging
+
 from vernie import *
 
+logging.basicConfig(level=logging.DEBUG)
+
 robot = Vernie()
 running = True