From 943823e9bc18e750ab2a06c7c2ccc427bc80627b Mon Sep 17 00:00:00 2001
From: Emil Velikov <emil.velikov@festo.com>
Date: Thu, 4 Nov 2021 13:53:11 +0200
Subject: [PATCH 1/2] Python: Add conversion between signed and unsigned and
 vice versa.

Fixes #28
---
 examples/python/src/driver/drvPgva.py | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/examples/python/src/driver/drvPgva.py b/examples/python/src/driver/drvPgva.py
index c5e88a5..663377d 100644
--- a/examples/python/src/driver/drvPgva.py
+++ b/examples/python/src/driver/drvPgva.py
@@ -97,13 +97,25 @@ def readData(self, register, sign=True):
         data = 0
         try:
             data = self.client.read_input_registers(register, 1, unit=self.pgvaConfig['modbusSlave'])
-            return data.registers[0]
         except Exception as e:
             print("Error while reading : ", str(e))
 
+        result = data.registers[0]
+
+        if (sign):
+            if (result & 0x8000):
+                result = -(~result & 0xFFFF) - 1
+
+        return result
+
+
 
     def writeData(self, register, val, sign=True):
         status = object
+
+        if (sign):
+            val &= 0xFFFF
+
         print(f"{register}, {val}")
         try:
             if val < 0:

From 9352738821db62e6d78580fd0f7d9e2d53deca37 Mon Sep 17 00:00:00 2001
From: Emil Velikov <emil.velikov@festo.com>
Date: Thu, 4 Nov 2021 13:56:22 +0200
Subject: [PATCH 2/2] Python: Wait until the desired pressure is regulated
 instead of fixed delay.

Fixes #28
---
 examples/python/src/driver/drvPgva.py | 41 +++++++++++++++++++++++++--
 1 file changed, 38 insertions(+), 3 deletions(-)

diff --git a/examples/python/src/driver/drvPgva.py b/examples/python/src/driver/drvPgva.py
index 663377d..ab32da4 100644
--- a/examples/python/src/driver/drvPgva.py
+++ b/examples/python/src/driver/drvPgva.py
@@ -169,7 +169,7 @@ def aspirate(self, actuationTime: int, pressure: int):
             self.writeData(_ModbusCommands.OutputPressuremBar, pressure)
         else:
             raise ValueError("Pressure Data not in range")
-        time.sleep(0.5)
+        self._waitUntilPressureRegulated(pressure)
         #set actuation time
         if actuationTime in range(0, 1000):
             self.writeData(_ModbusCommands.ValveActuationTime, actuationTime, sign=False)
@@ -183,7 +183,7 @@ def dispense(self, actuationTime: int, pressure: int):
             self.writeData(_ModbusCommands.OutputPressuremBar, pressure)
         else:
             raise ValueError("Pressure Data not in range")
-        time.sleep(0.5)
+        self._waitUntilPressureRegulated(pressure)
         #set actuation time
         if actuationTime in range(0, 1000):
             self.writeData(_ModbusCommands.ValveActuationTime, actuationTime, sign=False)
@@ -197,7 +197,42 @@ def readSensData(self):
         self.sensorData['pressureChamber'] = self.readData(_ModbusCommands.PressureActualmBar, True)
         self.sensorData['outputPressure'] = self.readData(_ModbusCommands.OutputPressureActualmBar, True)
     
-        return self.sensorData       
+        return self.sensorData   
+
+    def _waitUntilPressureRegulated(self, desiredPressure:int):
+        STABLE_PRESSURE_THRESHOLD = 3
+        TIMEOUT = 100 # 100 * 50 ms = 5000 ms
+
+        timesPressureReached = 0
+        timeoutCounter = 0
+
+        # If the desired pressure less than +-50 mbar we will wait until the
+        # pressure is +- 10%. Otherwise, wait until the pressure is within +- 5%
+        if (abs(desiredPressure) <= 50):
+            margin = 0.1
+        else:
+            margin = 0.05
+
+        maxAllowedDeviation = abs(int(desiredPressure * margin))
+        
+    
+        while True:
+            actualPressure = self.readData(_ModbusCommands.OutputPressureActualmBar, True)
+            if (abs(desiredPressure - actualPressure) < maxAllowedDeviation):
+                timesPressureReached += 1
+            else:
+                timesPressureReached = 0
+            
+            if (timesPressureReached == STABLE_PRESSURE_THRESHOLD):
+                break
+            
+            if (timeoutCounter > TIMEOUT):
+                return False
+
+            timeoutCounter += 1
+
+        return True
+