diff --git a/arduino/hev_prototype_v1/src/BreathingLoop.cpp b/arduino/hev_prototype_v1/src/BreathingLoop.cpp
index a8134fdca91a1443feb7e33abfdfd8561163fcb9..ce24a5a33172c6b53ccae19fd108f6d59b38d7ce 100644
--- a/arduino/hev_prototype_v1/src/BreathingLoop.cpp
+++ b/arduino/hev_prototype_v1/src/BreathingLoop.cpp
@@ -31,7 +31,7 @@ BreathingLoop::BreathingLoop()
_volume = 0;
_airway_pressure = 0;
- _pid.Kp = 0.1; // proportional factor
+ _pid.Kp = 0.007; // proportional factor
_pid.Ki = 0; // integral factor
_pid.Kd = 0; // derivative factor
}
@@ -94,17 +94,17 @@ void BreathingLoop::updateReadings()
float_t _pressure_inhale = adcToMillibarFloat((_readings_sums.pressure_inhale / _readings_N), _calib_avgs.pressure_inhale );
- float output = 0.;
-
doPID(10., _pressure_inhale, _valve_inhale_PID_percentage, _airway_pressure, _volume, _flow);
- _valve_inhale_PID_percentage /= 10.; // In the Labview code the output was defined from 0-10V. It is a simple rescale to keep the same parameters
+ _volume = _valve_inhale_PID_percentage;
+
+ //_valve_inhale_PID_percentage /= 10.; // In the Labview code the output was defined from 0-10V. It is a simple rescale to keep the same parameters
//Lazy approach
//airway_pressure = Proportional
//volume = Integral
//flow = Derivative
- //_valves_controller.setValves(VALVE_STATE::CLOSED, VALVE_STATE::CLOSED, _valve_inhale_PID_percentage*VALVE_STATE::OPEN, VALVE_STATE::CLOSED, VALVE_STATE::CLOSED);
-
+ _valves_controller.setPIDoutput(_valve_inhale_PID_percentage);
+ _valves_controller.setValves(VALVE_STATE::CLOSED, VALVE_STATE::CLOSED, VALVE_STATE::PID, VALVE_STATE::CLOSED, VALVE_STATE::CLOSED);
}
@@ -342,7 +342,7 @@ void BreathingLoop::FSM_breathCycle()
} else {
_fsm_timeout = 1000;
}
- _valves_controller.setValves(VALVE_STATE::CLOSED, VALVE_STATE::CLOSED, VALVE_STATE::CLOSED, VALVE_STATE::CLOSED, VALVE_STATE::CLOSED);
+ _valves_controller.setValves(VALVE_STATE::CLOSED, VALVE_STATE::CLOSED, VALVE_STATE::FULLY_CLOSED, VALVE_STATE::CLOSED, VALVE_STATE::CLOSED);
initCalib();
break;
case BL_STATES::CALIBRATION :
@@ -399,7 +399,7 @@ void BreathingLoop::FSM_breathCycle()
// TODO : spontaneous trigger
// if p_inhale > max thresh pressure(def: 50?)
// go to exhale fill
- _valves_controller.setValves(VALVE_STATE::CLOSED, VALVE_STATE::CLOSED, VALVE_STATE::OPEN, VALVE_STATE::CLOSED, VALVE_STATE::CLOSED);
+ //_valves_controller.setValves(VALVE_STATE::CLOSED, VALVE_STATE::CLOSED, VALVE_STATE::OPEN, VALVE_STATE::CLOSED, VALVE_STATE::CLOSED);//Comment this line for the PID control during inhale
_fsm_timeout = _states_durations.inhale;
break;
@@ -435,7 +435,7 @@ void BreathingLoop::FSM_breathCycle()
break;
case BL_STATES::STOP:
// TODO : require a reset command to go back to idle
- _valves_controller.setValves(VALVE_STATE::CLOSED, VALVE_STATE::CLOSED, VALVE_STATE::CLOSED, VALVE_STATE::CLOSED, VALVE_STATE::CLOSED);
+ _valves_controller.setValves(VALVE_STATE::CLOSED, VALVE_STATE::CLOSED, VALVE_STATE::FULLY_CLOSED, VALVE_STATE::CLOSED, VALVE_STATE::CLOSED);
_fsm_timeout = 1000;
break;
}
@@ -562,8 +562,15 @@ void BreathingLoop::doPID(float target_pressure, float process_pressure, float &
proportional = _pid.Kp*error;
//TODO integral and derivative
+ //
+
+ float minimum_open_frac = 0.54; //Minimum opening to avoid vibrations on the valve control
+ float maximum_open_frac = 0.64; //Maximum opening for the PID control
+
+ output = proportional + minimum_open_frac;
- output = proportional;
+ if(output > maximum_open_frac) output = maximum_open_frac;
+ if(output < minimum_open_frac) output = minimum_open_frac;
}
pid_variables& BreathingLoop::getPIDVariables()
diff --git a/arduino/hev_prototype_v1/src/ValvesController.cpp b/arduino/hev_prototype_v1/src/ValvesController.cpp
index 8364cc4a618537e036ef1439441cdff7087a6ea5..f12eb5e070e9fab7ca11e45eea119760fb68e1a5 100644
--- a/arduino/hev_prototype_v1/src/ValvesController.cpp
+++ b/arduino/hev_prototype_v1/src/ValvesController.cpp
@@ -30,7 +30,7 @@ ValvesController::ValvesController()
_inhale_duty_cycle = 0;
_inhale_open_max = MAX_VALVE_FRAC_OPEN;
- _inhale_open_min = 0;
+ _inhale_open_min = 0.54;
_valve_inhale_percent = 0; // replaced by a min level and a max level; bias inhale level. very slightly open at "closed" position
_valve_exhale_percent = 0;
@@ -39,6 +39,8 @@ ValvesController::ValvesController()
_valve_purge_enable = 1;
_inhale_trigger_enable = 0; // params - associated val of peak flow
_exhale_trigger_enable = 0;
+
+ _PID_output = 0;
}
ValvesController::~ValvesController()
@@ -127,7 +129,7 @@ void ValvesController::setValves(bool vin_air, bool vin_o2, uint8_t vinhale,
case VALVE_STATE::PID:
// placeholder - this should be replaced by:
//doPID(_inhale.pin);
- setPWMValve(_inhale.pin, _inhale_open_max);
+ setPWMValve(_inhale.pin, _PID_output);//_inhale_open_max);
break;
default:
break;
@@ -197,3 +199,10 @@ void ValvesController::enableAirInValve(bool en)
_valve_air_in_enable = en;
}
+void ValvesController::setPIDoutput(float value){
+ _PID_output = value;
+}
+
+float ValvesController::getPIDoutput(){
+ return _PID_output;
+}
diff --git a/arduino/hev_prototype_v1/src/ValvesController.h b/arduino/hev_prototype_v1/src/ValvesController.h
index 4ebb93f7602bc87144ab79f7c3ce9d03196c9fc2..77fb283fc4d549cdef5ff1d47fa4be2bbd8b7f08 100644
--- a/arduino/hev_prototype_v1/src/ValvesController.h
+++ b/arduino/hev_prototype_v1/src/ValvesController.h
@@ -48,6 +48,9 @@ public:
void setInhaleOpenMin(uint32_t value);
void setInhaleOpenMax(uint32_t value);
+ void setPIDoutput(float value);
+ float getPIDoutput();
+
private:
valve _air_in;
valve _o2_in;
@@ -67,6 +70,8 @@ private:
float _inhale_duty_cycle;
float _inhale_open_min;
float _inhale_open_max;
+
+ float _PID_output;
};
-#endif
\ No newline at end of file
+#endif
diff --git a/arduino/hev_prototype_v1/src/common.cpp b/arduino/hev_prototype_v1/src/common.cpp
index 789e78fc28c684e45bc3f0a5c90f50d1f02cf318..28707e8ab315edc51dd75e2784a0000ded79999e 100644
--- a/arduino/hev_prototype_v1/src/common.cpp
+++ b/arduino/hev_prototype_v1/src/common.cpp
@@ -153,13 +153,13 @@ void setPID(CMD_SET_PID cmd, pid_variables &pid, uint32_t &value)
{
switch(cmd){
case CMD_SET_PID::KP:
- pid.Kp = value/1000.0;
+ pid.Kp = value/1000000.0;
break;
case CMD_SET_PID::KI:
- pid.Ki = value/1000.0;
+ pid.Ki = value/1000000.0;
break;
case CMD_SET_PID::KD:
- pid.Kd = value/1000.0;
+ pid.Kd = value/1000000.0;
break;
default:
break;
@@ -207,6 +207,12 @@ float_t adcToMillibarFloat(int16_t adc, int16_t offset = 0)
float c = max_p - m * max_adc;
float mbar = m*(adc-offset) + c;
+ float PCB_Gain = 2. ; // real voltage is two times higher thant the measured in the PCB (there is a voltage divider)
+ float Sensor_Gain = 400./4000. ; // the sensor gain is 400 mbar / 4000 mVolts
+ float ADC_to_Voltage_Gain = 3300./4096.0 ; // maximum Voltage of 3.3V for 4096 ADC counts - (It might need recalibration?)
+
+ mbar = PCB_Gain * Sensor_Gain * ADC_to_Voltage_Gain * (adc - offset); // same calculation as in the Labview Code
+
return static_cast<float_t>(mbar);
//return static_cast<int16_t>(adc);
}
diff --git a/arduino/hev_prototype_v1/src/main.cpp b/arduino/hev_prototype_v1/src/main.cpp
index b21530ed1785f221b4e79fe6d44c8203f66c4b96..df0869dec204dc8a23450498dba70c58f5f2efb3 100644
--- a/arduino/hev_prototype_v1/src/main.cpp
+++ b/arduino/hev_prototype_v1/src/main.cpp
@@ -36,8 +36,8 @@ void setup()
#ifdef CHIP_ESP32
WiFi.mode(WIFI_OFF);
btStop();
- ledcSetup(pwm_chan_inhale, pwm_frequency, pwm_resolution);
- ledcSetup(pwm_chan_exhale, pwm_frequency, pwm_resolution);
+ ledcSetup(pwm_chan_inhale, 500, pwm_resolution); // 500 Hz for proportional valves
+ ledcSetup(pwm_chan_exhale, 500, pwm_resolution); // 500 Hz for proportional valves
ledcSetup(3, 2000, pwm_resolution);
ledcAttachPin(pin_buzzer, 3);
ledcAttachPin(pin_valve_inhale , pwm_chan_inhale);
diff --git a/raspberry-dataserver/CommsDebug.py b/raspberry-dataserver/CommsDebug.py
index 253fbd4c31c7dc9d9577923dc34446c802698f77..6eb580bf0a5dec808cd5a9c030ff333ecc59c46e 100755
--- a/raspberry-dataserver/CommsDebug.py
+++ b/raspberry-dataserver/CommsDebug.py
@@ -31,10 +31,10 @@ class Dependant(object):
self._lli.bind_to(self.update_llipacket)
def update_llipacket(self, payload):
- #logging.info(f"payload received: {payload}")
- if payload.getType() == 1:
- logging.info(f"payload received: {payload}")
- #logging.info(f"Fsm state: {payload.fsm_state}")
+ logging.info(f"payload received: {payload}")
+ #if payload.getType() == 1:
+ # logging.info(f"payload received: {payload}")
+ # #logging.info(f"Fsm state: {payload.fsm_state}")
#if hasattr(payload, 'ventilation_mode'):
# logging.info(f"payload received: {payload.ventilation_mode}")
#if hasattr(payload, 'duration_inhale'):
@@ -46,17 +46,22 @@ class Dependant(object):
async def commsDebug():
#cmd = CommandFormat(cmd_type=CMD_TYPE.GENERAL.value, cmd_code=CMD_GENERAL.START.value, param=0)
#cmd = CommandFormat(cmd_type=CMD_TYPE.SET_TIMEOUT.value, cmd_code=CMD_SET_TIMEOUT.INHALE.value, param=1111)
+
+ cmd = CommandFormat(cmd_type=CMD_TYPE.SET_PID.value, cmd_code=CMD_SET_PID.KP.value, param=20000) # to set Kp=0.0002, param=200 i.e., micro_Kp
+ await asyncio.sleep(1)
+ comms.writePayload(cmd)
+ await asyncio.sleep(1)
cmd = CommandFormat(cmd_type=CMD_TYPE.GENERAL.value, cmd_code=CMD_GENERAL.START.value, param=0)
await asyncio.sleep(1)
comms.writePayload(cmd)
print('sent cmd start')
toggle = 2
while True:
- await asyncio.sleep(15)
- #cmd = CommandFormat(cmd_type=CMD_TYPE.SET_PID.value, cmd_code=CMD_SET_PID.KP.value, param=200) # to set Kp=0.2, param=200 i.e., milli_Kp
+ await asyncio.sleep(60)
+ #cmd = CommandFormat(cmd_type=CMD_TYPE.SET_PID.value, cmd_code=CMD_SET_PID.KP.value, param=5) # to set Kp=0.2, param=200 i.e., milli_Kp
#comms.writePayload(cmd)
#print('sent cmd set Kp = 0.2')
- await asyncio.sleep(15)
+ await asyncio.sleep(60)
cmd = CommandFormat(cmd_type=CMD_TYPE.GENERAL.value, cmd_code=toggle, param=0)
if toggle == 2 :
toggle = 1
diff --git a/raspberry-dataserver/CommsDebugWithPlots.py b/raspberry-dataserver/CommsDebugWithPlots.py
new file mode 100755
index 0000000000000000000000000000000000000000..87c42f497986b4f954f9a8400aa663ca372c62bc
--- /dev/null
+++ b/raspberry-dataserver/CommsDebugWithPlots.py
@@ -0,0 +1,225 @@
+#!/usr/bin/env python3
+from CommsLLI import CommsLLI
+from CommsCommon import *
+import asyncio
+import logging
+logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s')
+from serial.tools import list_ports
+import sys
+import time
+
+import matplotlib.pyplot as plt
+import numpy as np
+import queue
+
+plt.ion()
+
+history_length = 5000
+
+pressure_buffer = queue.Queue(history_length)
+pressure_inhale = queue.Queue(history_length)
+PID_P = queue.Queue(history_length)
+PID_I = queue.Queue(history_length)
+PID_D = queue.Queue(history_length)
+
+pressure_buffer = queue.Queue(history_length)
+pressure_inhale = queue.Queue(history_length)
+PID_P = queue.Queue(history_length)
+PID_I = queue.Queue(history_length)
+PID_D = queue.Queue(history_length)
+
+fig = plt.figure()
+
+ax = fig.add_subplot(111)
+
+h1, = ax.plot([],[], label="buffer")
+h2, = ax.plot([],[], label="inhale")
+h3, = ax.plot([],[], label="Proportional")
+h4, = ax.plot([],[], label="Integral")
+h5, = ax.plot([],[], label="Derivative")
+#plt.axes()
+#an = []
+
+#for i in range(history_length):
+# pressure_buffer.put(-1)
+# pressure_inhale.put(-1)
+# PID_P.put(-1)
+# PID_I.put(-1)
+# PID_D.put(-1)
+
+last_data = 0
+last_build_time = time.time()
+
+async def draw_plots():
+
+ while True:
+
+ await asyncio.sleep(1)
+
+ if(pressure_inhale.qsize() == 0): continue
+
+ print("Running draw plots finished ", pressure_inhale.qsize())
+
+ h1.set_xdata(np.array(range(pressure_inhale.qsize())))
+ h1.set_ydata(list(pressure_inhale.queue))#list(pressure_inhale.queue))
+
+ h2.set_xdata(np.array(range(pressure_buffer.qsize())))
+ h2.set_ydata(list(pressure_buffer.queue))#list(pressure_buffer.queue))
+
+ h3.set_xdata(np.array(range(PID_P.qsize())))
+ h3.set_ydata(list(PID_P.queue))#list(pressure_buffer.queue))
+
+ h4.set_xdata(np.array(range(PID_I.qsize())))
+ h4.set_ydata(list(PID_I.queue))#list(pressure_buffer.queue))
+
+ h5.set_xdata(np.array(range(PID_D.qsize())))
+ h5.set_ydata(list(PID_D.queue))#list(pressure_buffer.queue))
+
+ plt.legend()
+ #plt.ylim(-2,20)
+
+ ax.relim()
+ ax.autoscale_view(True,True,True)
+ fig.canvas.draw()
+ fig.canvas.flush_events()
+ #time.sleep(60)
+ #time.sleep(0.1)
+
+ #ai, = ax.plot(range(10), range(10))
+ #an.append(ai)
+
+ fig.canvas.draw()
+ plt.show(block=False)
+
+
+def FILO(_queue, newitem):
+ if _queue.full(): _queue.get()
+ _queue.put(newitem)
+
+async def build_history_plots():
+
+ """
+ 2020-05-06 10:45:55,948 - INFO - payload received: DataFormat(version=163, timestamp=231682, payload_type=<PAYLOAD_TYPE.DATA: 1>, fsm_state=<BL_STATES.STOP: 11>, pressure_air_supply=41, pressure_air_regulated=453.0, pressure_o2_supply=30, pressure_o2_regulated=451.0, pressure_buffer=242.0, pressure_inhale=0.0, pressure_patient=0.0, temperature_buffer=659, pressure_diff_patient=1331.0, ambient_pressure=0, ambient_temperature=0, airway_pressure=-0.14404296875, flow=0.0, volume=0.0)
+{'version': 163, 'timestamp': 231682, 'payload_type': 'DATA', 'fsm_state': 'STOP', 'pressure_air_supply': 41, 'pressure_air_regulated': 453.0, 'pressure_o2_supply': 30, 'pressure_o2_regulated': 451.0, 'pressure_buffer': 242.0, 'pressure_inhale': 0.0, 'pressure_patient': 0.0, 'temperature_buffer': 659, 'pressure_diff_patient': 1331.0, 'ambient_pressure': 0, 'ambient_temperature': 0, 'airway_pressure': -0.14404296875, 'flow': 0.0, 'volume': 0.0}
+0.0
+2020-05-06 10:45:55,959 - INFO - payload received: DataFormat(version=163, timestamp=231737, payload_type=<PAYLOAD_TYPE.DATA: 1>, fsm_state=<BL_STATES.STOP: 11>, pressure_air_supply=48, pressure_air_regulated=453.0, pressure_o2_supply=30, pressure_o2_regulated=452.0, pressure_buffer=242.0, pressure_inhale=0.0, pressure_patient=0.0, temperature_buffer=659, pressure_diff_patient=1325.0, ambient_pressure=0, ambient_temperature=0, airway_pressure=-0.14404296875, flow=0.0, volume=0.0)
+{'version': 163, 'timestamp': 231737, 'payload_type': 'DATA', 'fsm_state': 'STOP', 'pressure_air_supply': 48, 'pressure_air_regulated': 453.0, 'pressure_o2_supply': 30, 'pressure_o2_regulated': 452.0, 'pressure_buffer': 242.0, 'pressure_inhale': 0.0, 'pressure_patient': 0.0, 'temperature_buffer': 659, 'pressure_diff_patient': 1325.0, 'ambient_pressure': 0, 'ambient_temperature': 0, 'airway_pressure': -0.14404296875, 'flow': 0.0, 'volume': 0.0}
+0.0
+ """
+
+ print("Starting Build data")
+
+ global last_build_time
+
+ while True:
+
+ if time.time() - last_build_time < 0.1 or last_build_time == 0:
+ continue
+
+ last_build_time = time.time()
+
+ #if last_data == 0: continue
+
+ try:
+
+ _pressure_inhale = last_data["pressure_inhale"]
+ _pressure_buffer = last_data["pressure_buffer"]
+ _PID_P = last_data["airway_pressure"]
+ _PID_I = last_data["volume"]
+ _PID_D = last_data["flow"]
+
+ FILO(pressure_inhale, _pressure_inhale)
+ FILO(pressure_buffer, _pressure_buffer)
+ FILO(PID_P, _PID_P)
+ FILO(PID_I, _PID_I)
+ FILO(PID_D, _PID_D)
+ print("data acquired")
+
+ except KeyError:
+ pass
+
+
+def getTTYPort():
+ port_device = ""
+ for port in list_ports.comports():
+ vidpid = ""
+ if port.pid != None and port.vid != None:
+ vidpid = f"{ port.vid:04x}:{port.pid:04x}".upper()
+ print(vidpid)
+ if port.manufacturer and "ARDUINO" in port.manufacturer.upper():
+ port_device = port.device
+ elif vidpid == "10C4:EA60" :
+ port_device = port.device
+ elif len(sys.argv) > 1:
+ port_device = sys.argv[1]
+ return port_device
+
+
+class Dependant(object):
+ def __init__(self, lli):
+ self._llipacket = None
+ self._lli = lli
+ self._lli.bind_to(self.update_llipacket)
+
+ def update_llipacket(self, payload):
+ global last_data
+ #logging.info(f"payload received: {payload}")
+ if payload.getType() == 1:
+ logging.info(f"payload received: {payload}")
+ #logging.info(f"Fsm state: {payload.fsm_state}")
+ #if hasattr(payload, 'ventilation_mode'):
+ # logging.info(f"payload received: {payload.ventilation_mode}")
+ #if hasattr(payload, 'duration_inhale'):
+ # logging.info(f"payload received: inhale duration = {payload.duration_inhale} ")
+ self._llipacket = payload.getDict() # returns a dict
+ last_data = self._llipacket #= payload.getDict() # returns a dict
+
+ #build_history_plots(payload.getDict())
+ #if self.counter % 10 == 0: draw_plots()
+
+# initialise as start command, automatically executes toByteArray()
+async def commsDebug():
+ #cmd = CommandFormat(cmd_type=CMD_TYPE.GENERAL.value, cmd_code=CMD_GENERAL.START.value, param=0)
+ #cmd = CommandFormat(cmd_type=CMD_TYPE.SET_TIMEOUT.value, cmd_code=CMD_SET_TIMEOUT.INHALE.value, param=1111)
+ cmd = CommandFormat(cmd_type=CMD_TYPE.GENERAL.value, cmd_code=CMD_GENERAL.START.value, param=0)
+ await asyncio.sleep(1)
+ comms.writePayload(cmd)
+ print('sent cmd start')
+ toggle = 2
+ while True:
+ await asyncio.sleep(15)
+ #cmd = CommandFormat(cmd_type=CMD_TYPE.SET_PID.value, cmd_code=CMD_SET_PID.KP.value, param=200) # to set Kp=0.2, param=200 i.e., milli_Kp
+ #comms.writePayload(cmd)
+ #print('sent cmd set Kp = 0.2')
+ await asyncio.sleep(15)
+ cmd = CommandFormat(cmd_type=CMD_TYPE.GENERAL.value, cmd_code=toggle, param=0)
+ if toggle == 2 :
+ toggle = 1
+ else :
+ toggle = 2
+
+ comms.writePayload(cmd)
+ print('sent cmd stop')
+
+try:
+ # setup serial device and init server
+ loop = asyncio.get_event_loop()
+ comms = CommsLLI(loop)
+ dep = Dependant(comms)
+
+ # create tasks
+ lli = comms.main(getTTYPort(), 115200)
+ debug = commsDebug()
+ plot = draw_plots()
+ getdata = build_history_plots()
+ tasks = [lli, debug, getdata]#, plot]
+
+ # run tasks
+ asyncio.gather(*tasks, return_exceptions=True)
+ loop.run_forever()
+except asyncio.CancelledError:
+ pass
+except KeyboardInterrupt:
+ logging.info("Closing LLI")
+finally:
+ loop.close()