Wait for pressure to be regulated instead of fixed delay

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:
@@ -157,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)
@@ -171,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)
@@ -185,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
+