From 2e2e36d560ef7263d6113e7f1a31de77aaf076d4 Mon Sep 17 00:00:00 2001
From: Andrey Pohilko <apc4@ya.ru>
Date: Sun, 24 Dec 2017 11:32:42 +0300
Subject: [PATCH] Protect motor from going nuts in angled mode

---
 pylgbst/peripherals.py | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/pylgbst/peripherals.py b/pylgbst/peripherals.py
index f7a1064..0cfdfbc 100644
--- a/pylgbst/peripherals.py
+++ b/pylgbst/peripherals.py
@@ -172,11 +172,17 @@ class EncodedMotor(Peripheral):
         if self.port == PORT_AB:
             mtype += 1  # de-facto rule
 
+        abs_primary = self._speed_abs(speed_primary)
+        abs_secondary = self._speed_abs(speed_secondary)
+
+        if mtype == self.ANGLED_GROUP and (not abs_secondary or not abs_primary):
+            raise ValueError("Cannot have zero speed in double angled mode")  # otherwise it gets nuts
+
         params = pack("<B", self.MOVEMENT_TYPE) + pack("<B", mtype) + params
 
-        params += pack("<b", self._speed_abs(speed_primary))
+        params += pack("<b", abs_primary)
         if self.port == PORT_AB:
-            params += pack("<b", self._speed_abs(speed_secondary))
+            params += pack("<b", abs_secondary)
 
         params += self.TRAILER