Merge pull request #48 from DEIS-Tools/fw-ready-signal

Add ready signal

examples/Console/Console.ino

@@ -46,14 +46,20 @@ void attachCommandCallbacks() {
   cmdMessenger.attach(kVerbosity, OnVerbosity);
 }
 
+void OnReady() {
+  Serial.println("CLAIRE-READY");
+}
+
 // Called when a received command has no attached function
 void OnUnknownCommand() {
   Serial.println("This command is unknown!");
   ShowCommands();
+  OnReady();
 }
 
 void OnCommandList() {
   ShowCommands();
+  OnReady();
 }
 
 String getStatus(bool filtered) {
@@ -79,16 +85,19 @@ String getStatus(bool filtered) {
 void OnStatus() {
   // do default samples
   Serial.println(getStatus(true));
+  OnReady();
 }
 
 void OnQuickStatus() {
   // only do one sample
   Serial.println(getStatus(false));
+  OnReady();
 }
 
 void OnStop() {
   claire.eStop();
   Serial.println("Emergency stop applied! Expect state of demonstrator to be undefined.");
+  OnReady();
 }
 
 void OnSetPump() {
@@ -124,6 +133,7 @@ void OnSetPump() {
       Serial.println("Unknown pump referenced (" + String(tube) + "), ignoring command");
       return;
   }
+  OnReady();
 }
 
 // sets water level in a tube to arg
@@ -155,7 +165,7 @@ void OnSetLevel() {
   }
 
   // Notify that command is finished.
-  Serial.println("Finished");
+  OnReady();
 }
 
 void OnPrime() {
@@ -188,7 +198,7 @@ void OnPrime() {
   }
 
   // Notify that command is finished.
-  Serial.println("Finished");
+  OnReady();
 }
 
 // empty all containers into res.
@@ -201,6 +211,7 @@ void OnReset() {
   if (ok_tube1 && ok_tube2) {
     Serial.println("All tubes emptied successfully");
   }
+  OnReady();
 }
 
 
@@ -209,10 +220,12 @@ void OnEmpty() {
   OnReset();
   Serial.println("WARN: Running pump: " + String(TUBE1_IN.name) + " at 100% until user resets!");
   claire.setPump(TUBE1_IN, 100);
+  OnReady();
 }
 
 void OnTest() {
   claire.testOutput();
+  OnReady(); // not reachable unless err
 }
 
 void OnVerbosity() {
@@ -221,6 +234,7 @@ void OnVerbosity() {
   claire.DEBUG = DEBUG;
   claire.VERBOSE = VERBOSE;
   Serial.println("Verbose: " + String(VERBOSE) + " debug: " + String(DEBUG));
+  OnReady();
 }
 
 // Show available commands
@@ -259,6 +273,9 @@ void setup() {
 
   // Show command list
   ShowCommands();
+
+  // Give ready signal when init is finished
+  OnReady();
 }
 
 // Loop function

library.properties

@@ -1,5 +1,5 @@
 name=CLAIRE
-version=0.1.11
+version=0.1.12
 author=Falke Carlsen <falkeboc@cs.aau.dk>
 maintainer=Falke Carlsen <falkeboc@cs.aau.dk>
 sentence=API to interface with CLAIRE water management demonstrator at DEIS-AAU.

py_driver/driver.py

@@ -1,15 +1,17 @@
 import sys
 import threading
+from datetime import timedelta
 from time import sleep, time
 import serial
 import utils
 
 IMMEDIATE_OUTPUT = True
 TAG = "DRIVER:"
-CLAIRE_VERSION = "v0.1.11"
+CLAIRE_VERSION = "v0.1.12"
 TUBE_MAX_LEVEL = 900
 DEBUG = True
 COMMUNICATION_TIMEOUT = 10
+UNDERFLOW_CHECK_INTERVAL = 5
 
 
 class SensorError(Exception):
@@ -49,9 +51,11 @@ class ClaireState:
     """
     The state of the Claire demonstrator. Can be used to cache the state.
     """
+
     def __init__(self):
         self.state = None
         self.outdated = True
+        self.last_update = 0
 
     def set_state(self, state):
         """
@@ -60,7 +64,8 @@ def set_state(self, state):
         :param state: The new state to cache.
         """
         self.state = state
-        self.outdated = state["Tube1_inflow_duty"] or state["Tube1_outflow_duty"] or state["Tube2_inflow_duty"] or state["Tube2_outflow_duty"]
+        self.outdated = state["Tube1_inflow_duty"] or state["Tube1_outflow_duty"] or state["Tube2_inflow_duty"] or \
+                        state["Tube2_outflow_duty"]
 
     def make_outdated(self):
         """Label the cached state as outdated."""
@@ -71,10 +76,11 @@ class ClaireDevice:
     """
     Class that represents the Claire demonstrator setup.
     """
+
     def __init__(self, port):
         self.device = port
         self.heartbeat = time()
-        self.busy = False
+        self.busy = True  # initially unknown, therefore busy
         self.state = ClaireState()
         # read timeout in secs, 1 should be sufficient
 
@@ -90,13 +96,64 @@ def __init__(self, port):
         self.stopped = False
         self.read_buffer = []
         self.last_printed_buf_line = -1
+
         self.read_thread = threading.Thread(target=self._read_lines)
         self.read_thread.daemon = True
         self.read_thread.start()
+
+        self.underflow_thread = threading.Thread(target=self._underflow_check)
+        self.underflow_thread.daemon = True
+        self.underflow_thread.start()
+
         print(f'{TAG} Device connected to {port}, waiting for initialization...')
-        sleep(3)
+        while not self.ready():
+            sleep(1)
         self.check_version()
 
+    def alive(self):
+        """Check if the device is still alive within bound."""
+        return time() - self.heartbeat < COMMUNICATION_TIMEOUT
+
+    def ready(self):
+        return not self.busy
+
+    def _underflow_check(self):
+        TAG = "UNDERFLOW_CHECK"
+        while True:
+            # sanity check
+            if not self.alive():
+                if DEBUG:
+                    print(f'{TAG}: Device is not alive. Waiting {UNDERFLOW_CHECK_INTERVAL} seconds.')
+                sleep(UNDERFLOW_CHECK_INTERVAL)
+                continue
+            if not self.ready():
+                if DEBUG:
+                    print(f'{TAG}: Device is busy. Waiting {UNDERFLOW_CHECK_INTERVAL} seconds.')
+                sleep(UNDERFLOW_CHECK_INTERVAL)
+                continue
+
+            # check if water level is below 0 fixme: errors out in callee during long-running functions due to timeout reached
+            self.update_state()
+
+            # check underflows
+            if self.state.state["Tube1_sonar_dist_mm"] < TUBE_MAX_LEVEL:
+                # if outflow is active while inflow is stopped, error out
+                if self.state.state["Tube1_outflow_duty"] > 0 and self.state.state["Tube1_inflow_duty"] == 0:
+                    self.set_outflow(1, 0)
+                    print(
+                        f'{TAG}: WARN: Low water level detected in tube 1: {self.state.state["Tube1_sonar_dist_mm"]}. Stopped outflow')
+
+            elif self.state.state["Tube2_sonar_dist_mm"] < TUBE_MAX_LEVEL:
+                # if outflow is active while inflow is stopped, error out
+                if self.state.state["Tube2_outflow_duty"] > 0 and self.state.state["Tube2_inflow_duty"] == 0:
+                    self.set_outflow(2, 0)
+                    print(
+                        f'{TAG}: WARN: Low water level detected in tube 2: {self.state.state["Tube2_sonar_dist_mm"]}. Stopped outflow')
+
+            else:
+                if DEBUG:
+                    print(f'{TAG}: No underflow detected in watchdog.')
+
     def _read_lines(self):
         """Read lines from the serial port and add to the buffer in a thread to not block the main thread."""
         while True:
@@ -107,9 +164,9 @@ def _read_lines(self):
                 self.read_buffer.extend(new_lines)
                 if IMMEDIATE_OUTPUT:
                     self.print_new_lines_buf()
-                # Check whether the new lines contain the finished signal.
+                # Check whether the new lines contain the ready signal.
                 for line in new_lines:
-                    if line == "Finished":
+                    if line == "CLAIRE-READY":
                         self.busy = False
 
             # Stop reading lines.
@@ -162,6 +219,8 @@ def print_new_lines_buf(self):
     def check_version(self):
         """Check the version of the software on the Arduino."""
         # Version number is on the first line, as that reads "Initialising CLAIRE water management <version>"
+        if len(self.read_buffer) == 0:
+            raise RuntimeError("No output received from device during version check.")
         line = self.read_buffer[0]
         words = line.split(' ')
 
@@ -173,10 +232,13 @@ def check_version(self):
         assert words[-1] == CLAIRE_VERSION, f"The CLAIRE software on the Arduino is version {words[-1]}, while this " \
                                             f"Python script is constructed for version {CLAIRE_VERSION}."
 
-    def get_state(self):
+        # check if device is ready
+        assert self.ready()
+
+    def update_state(self):
         """Get the last state of the device. If cached state is outdated, a new sensor reading is requested."""
         # Return cached state if not outdated.
-        if not self.state.outdated:
+        if not self.state.outdated and self.state.last_update >= time() - COMMUNICATION_TIMEOUT:
             return self.state.state
 
         # Ask for new state reading.
@@ -186,13 +248,19 @@ def get_state(self):
         # Wait for the state to be received.
         total_wait = 0
         while True:
-            if self.last_printed_buf_line > size_buffer and self.read_buffer[-1][0] == '{':
+            # Fixme: not robust
+            if self.last_printed_buf_line > size_buffer and self.read_buffer[-2][0] == '{':
                 # If we received a line starting with {, we have received the new state.
                 break
 
             sleep(0.1)
+            # do not incur waiting time if device is busy
+            if not self.ready():
+                continue
+
             total_wait += 0.1
-            if total_wait > COMMUNICATION_TIMEOUT:
+
+            if total_wait > COMMUNICATION_TIMEOUT and not self.busy:
                 raise RuntimeError("Waiting too long for state to be communicated.")
 
         # New state retrieved, parse it.
@@ -217,7 +285,12 @@ def get_last_raw_state(self):
 
     def print_state(self):
         """Print state of the system."""
-        print(f'{TAG} Got state: {self.state}')
+        # seconds since state was grabbed
+        if self.state.state:
+            old = timedelta(seconds=time() - self.state.last_update)
+            print(f'{TAG} State ({old} secs old): {self.state}')
+        else:
+            print(f'{TAG} State: N/A')
 
     def write(self, data):
         """
@@ -236,6 +309,7 @@ def close(self):
         """Close the serial connection."""
         self.stopped = True
         self.read_thread.join()  # Wait until read thread has been stopped.
+        self.underflow_thread.join()  # Wait until read thread has been stopped.
         self.ser.close()
 
     def set_water_level(self, tube, level):
@@ -247,6 +321,8 @@ def set_water_level(self, tube, level):
         """
         assert tube == 1 or tube == 2
         assert 0 <= level <= TUBE_MAX_LEVEL
+        while not self.ready():
+            sleep(1)
         self.write(f"5 {tube} {self.convert_level_to_distance(level)};")
         self.busy = True
         self.state.make_outdated()
@@ -260,6 +336,8 @@ def set_inflow(self, tube, rate):
         """
         assert tube == 1 or tube == 2
         assert 0 <= rate <= 100
+        while not self.ready():
+            sleep(1)
         pump = (tube - 1) * 2 + 1
         self.write(f"4 {pump} {rate};")
         self.state.make_outdated()
@@ -273,6 +351,8 @@ def set_outflow(self, tube, rate):
         """
         assert tube == 1 or tube == 2
         assert 0 <= rate <= 100
+        while not self.ready():
+            sleep(1)
         pump = tube * 2
         self.write(f"4 {pump} {rate};")
         self.state.make_outdated()

py_driver/experiment.py

@@ -9,8 +9,8 @@
 
 def example_experiment():
     claire = driver.ClaireDevice(PORT)
-    state = claire.get_state()  # get current state of device
-    claire.print_state(state)
+    state = claire.update_state()  # get current state of device
+    claire.print_state()
     print(f'Current height of TUBE1: {state["Tube1_sonar_dist_mm"]}')
 
     claire.set_inflow(1, 100)

src/Claire.h

@@ -4,7 +4,7 @@
 #include <Arduino.h>
 #include <EEPROM.h>
 
-#define VERSION "0.1.11"
+#define VERSION "0.1.12"
 
 #define OUTPUT_GPIO_MIN 2
 #define OUTPUT_GPIO_MAX 7