From adf5c09ca27b45282ab5b792c80dfb27399643e3 Mon Sep 17 00:00:00 2001
From: Dmitry Fink <finik@dishero.com>
Date: Mon, 14 Sep 2020 01:18:47 -0700
Subject: [PATCH] Handle failures on port out commands

Port out commands can fail (e.x obstruction of the movement), and it
will get the while executon stuck. The diff handles failures and
reports errors to the caller while at it, its useful to know when
the command fails.
---
 pylgbst/messages.py    |  5 ++++-
 pylgbst/peripherals.py | 37 +++++++++++++++++++++++++++----------
 2 files changed, 31 insertions(+), 11 deletions(-)

diff --git a/pylgbst/messages.py b/pylgbst/messages.py
index 733db90..9169df0 100644
--- a/pylgbst/messages.py
+++ b/pylgbst/messages.py
@@ -704,7 +704,7 @@ def bytes(self):
 
     def is_reply(self, msg):
         return isinstance(msg, MsgPortOutputFeedback) and msg.port == self.port \
-               and (msg.is_completed() or self.is_buffered)
+            and not (msg.is_in_progress() or msg.is_busy())
 
 
 class MsgPortOutputFeedback(UpstreamMsg):
@@ -736,6 +736,9 @@ def is_discarded(self):
     def is_idle(self):
         return self.status & 0b1000
 
+    def is_busy(self):
+        return self.status & 0b10000
+
 
 UPSTREAM_MSGS = (
     MsgHubProperties, MsgHubAction, MsgHubAlert, MsgHubAttachedIO, MsgGenericError,
diff --git a/pylgbst/peripherals.py b/pylgbst/peripherals.py
index 2f352ec..5f8052b 100644
--- a/pylgbst/peripherals.py
+++ b/pylgbst/peripherals.py
@@ -101,7 +101,7 @@ def set_port_mode(self, mode, send_updates=None, update_delta=None):
     def _send_output(self, msg):
         assert isinstance(msg, MsgPortOutput)
         msg.is_buffered = self.is_buffered  # TODO: support buffering
-        self.hub.send(msg)
+        return self.hub.send(msg)
 
     def get_sensor_data(self, mode):
         self.set_port_mode(mode)
@@ -275,12 +275,15 @@ def _write_direct_mode(self, subcmd, params):
         msg = MsgPortOutput(self.port, MsgPortOutput.WRITE_DIRECT_MODE_DATA, params)
         self._send_output(msg)
 
+    def _is_successful(self, msg):
+        return not msg.is_discarded()
+
     def _send_cmd(self, subcmd, params):
         if self.virtual_ports:
             subcmd += 1  # de-facto rule
 
         msg = MsgPortOutput(self.port, subcmd, params)
-        self._send_output(msg)
+        return self._send_output(msg)
 
     def start_power(self, power_primary=1.0, power_secondary=None):
         """
@@ -299,10 +302,12 @@ def start_power(self, power_primary=1.0, power_secondary=None):
         if self.virtual_ports:
             params += pack("<b", self._speed_abs(power_secondary))
 
-        self._send_cmd(cmd, params)
+        resp = self._send_cmd(cmd, params)
+
+        return self._is_successful(resp)
 
     def stop(self):
-        self.timed(0)
+        return self.timed(0)
 
     def set_acc_profile(self, seconds, profile_no=0x00):
         """
@@ -312,7 +317,9 @@ def set_acc_profile(self, seconds, profile_no=0x00):
         params += pack("<H", int(seconds * 1000))
         params += pack("<B", profile_no)
 
-        self._send_cmd(self.SUBCMD_SET_ACC_TIME, params)
+        resp = self._send_cmd(self.SUBCMD_SET_ACC_TIME, params)
+
+        return self._is_successful(resp)
 
     def set_dec_profile(self, seconds, profile_no=0x00):
         """
@@ -322,7 +329,9 @@ def set_dec_profile(self, seconds, profile_no=0x00):
         params += pack("<H", int(seconds * 1000))
         params += pack("<B", profile_no)
 
-        self._send_cmd(self.SUBCMD_SET_DEC_TIME, params)
+        resp = self._send_cmd(self.SUBCMD_SET_DEC_TIME, params)
+
+        return self._is_successful(resp)
 
     def start_speed(self, speed_primary=1.0, speed_secondary=None, max_power=1.0, use_profile=0b11):
         """
@@ -339,7 +348,9 @@ def start_speed(self, speed_primary=1.0, speed_secondary=None, max_power=1.0, us
         params += pack("<B", int(100 * max_power))
         params += pack("<B", use_profile)
 
-        self._send_cmd(self.SUBCMD_START_SPEED, params)
+        resp = self._send_cmd(self.SUBCMD_START_SPEED, params)
+
+        return self._is_successful(resp)
 
     def timed(self, seconds, speed_primary=1.0, speed_secondary=None, max_power=1.0, end_state=END_STATE_BRAKE,
               use_profile=0b11):
@@ -359,7 +370,9 @@ def timed(self, seconds, speed_primary=1.0, speed_secondary=None, max_power=1.0,
         params += pack("<B", end_state)
         params += pack("<B", use_profile)
 
-        self._send_cmd(self.SUBCMD_START_SPEED_FOR_TIME, params)
+        resp = self._send_cmd(self.SUBCMD_START_SPEED_FOR_TIME, params)
+
+        return self._is_successful(resp)
 
 
 class EncodedMotor(Motor):
@@ -400,7 +413,9 @@ def angled(self, degrees, speed_primary=1.0, speed_secondary=None, max_power=1.0
         params += pack("<B", end_state)
         params += pack("<B", use_profile)
 
-        self._send_cmd(self.SUBCMD_START_SPEED_FOR_DEGREES, params)
+        resp = self._send_cmd(self.SUBCMD_START_SPEED_FOR_DEGREES, params)
+
+        return self._is_successful(resp)
 
     def goto_position(self, degrees_primary, degrees_secondary=None, speed=1.0, max_power=1.0,
                       end_state=Motor.END_STATE_BRAKE, use_profile=0b11):
@@ -421,7 +436,9 @@ def goto_position(self, degrees_primary, degrees_secondary=None, speed=1.0, max_
         params += pack("<B", end_state)
         params += pack("<B", use_profile)
 
-        self._send_cmd(self.SUBCMD_GOTO_ABSOLUTE_POSITION, params)
+        resp = self._send_cmd(self.SUBCMD_GOTO_ABSOLUTE_POSITION, params)
+
+        return self._is_successful(resp)
 
     def _decode_port_data(self, msg):
         data = msg.payload