forked from M-Labs/kirdy
359 lines
13 KiB
Python
359 lines
13 KiB
Python
import math
|
|
import logging
|
|
from collections import deque, namedtuple
|
|
from enum import Enum
|
|
import socket
|
|
import json
|
|
import time
|
|
import signal
|
|
from driver.kirdy import Kirdy, FilterConfig
|
|
import asyncio
|
|
from sipyco.asyncio_tools import SignalHandler
|
|
|
|
# Based on hirshmann pid-autotune libiary
|
|
# See https://github.com/hirschmann/pid-autotune
|
|
# Which is in turn based on a fork of Arduino PID AutoTune Library
|
|
# See https://github.com/t0mpr1c3/Arduino-PID-AutoTune-Library
|
|
|
|
|
|
class PIDAutotuneState(Enum):
|
|
STATE_OFF = 'off'
|
|
STATE_RELAY_STEP_UP = 'relay step up'
|
|
STATE_RELAY_STEP_DOWN = 'relay step down'
|
|
STATE_SUCCEEDED = 'succeeded'
|
|
STATE_FAILED = 'failed'
|
|
STATE_READY = 'ready'
|
|
|
|
|
|
class PIDAutotune:
|
|
PIDParams = namedtuple('PIDParams', ['Kp', 'Ki', 'Kd'])
|
|
|
|
PEAK_AMPLITUDE_TOLERANCE = 0.05
|
|
|
|
_tuning_rules = {
|
|
"ziegler-nichols": [0.6, 1.2, 0.075],
|
|
"tyreus-luyben": [0.4545, 0.2066, 0.07214],
|
|
"ciancone-marlin": [0.303, 0.1364, 0.0481],
|
|
"pessen-integral": [0.7, 1.75, 0.105],
|
|
"some-overshoot": [0.333, 0.667, 0.111],
|
|
"no-overshoot": [0.2, 0.4, 0.0667]
|
|
}
|
|
|
|
def __init__(self, setpoint, out_step=10, lookback=60,
|
|
noiseband=0.5, sampletime=1.2):
|
|
if setpoint is None:
|
|
raise ValueError('setpoint must be specified')
|
|
|
|
self._inputs = deque(maxlen=round(lookback / sampletime))
|
|
self._sampletime = sampletime
|
|
self._setpoint = setpoint
|
|
self._outputstep = out_step
|
|
self._noiseband = noiseband
|
|
self._out_min = -out_step
|
|
self._out_max = out_step
|
|
self._state = PIDAutotuneState.STATE_OFF
|
|
self._peak_timestamps = deque(maxlen=5)
|
|
self._peaks = deque(maxlen=5)
|
|
self._output = 0
|
|
self._last_run_timestamp = 0
|
|
self._peak_type = 0
|
|
self._peak_count = 0
|
|
self._initial_output = 0
|
|
self._induced_amplitude = 0
|
|
self._Ku = 0
|
|
self._Pu = 0
|
|
|
|
def setParam(self, target, step, noiseband, sampletime, lookback):
|
|
self._setpoint = target
|
|
self._outputstep = step
|
|
self._out_max = step
|
|
self._out_min = -step
|
|
self._noiseband = noiseband
|
|
self._inputs = deque(maxlen=round(lookback / sampletime))
|
|
self._sampletime = sampletime
|
|
|
|
def setReady(self):
|
|
self._state = PIDAutotuneState.STATE_READY
|
|
self._peak_count = 0
|
|
|
|
def setOff(self):
|
|
self._state = PIDAutotuneState.STATE_OFF
|
|
|
|
def setFailed(self):
|
|
self._state = PIDAutotuneState.STATE_FAILED
|
|
self._peak_count = 30
|
|
|
|
def state(self):
|
|
"""Get the current state."""
|
|
return self._state
|
|
|
|
def output(self):
|
|
"""Get the last output value."""
|
|
return self._output
|
|
|
|
def tuning_rules(self):
|
|
"""Get a list of all available tuning rules."""
|
|
return self._tuning_rules.keys()
|
|
|
|
def get_tec_pid (self):
|
|
divisors = self._tuning_rules["tyreus-luyben"]
|
|
kp = self._Ku * divisors[0]
|
|
ki = divisors[1] * self._Ku / self._Pu / (1 / self._sampletime)
|
|
kd = divisors[2] * self._Ku * self._Pu * (1 / self._sampletime)
|
|
return kp, ki, kd
|
|
|
|
def get_pid_parameters(self, tuning_rule='ziegler-nichols'):
|
|
"""Get PID parameters.
|
|
|
|
Args:
|
|
tuning_rule (str): Sets the rule which should be used to calculate
|
|
the parameters.
|
|
"""
|
|
divisors = self._tuning_rules[tuning_rule]
|
|
kp = self._Ku * divisors[0]
|
|
ki = divisors[1] * self._Ku / self._Pu / (1 / self._sampletime)
|
|
kd = divisors[2] * self._Ku * self._Pu * (1 / self._sampletime)
|
|
return PIDAutotune.PIDParams(kp, ki, kd)
|
|
|
|
def run(self, input_val, time_input):
|
|
"""To autotune a system, this method must be called periodically.
|
|
|
|
Args:
|
|
input_val (float): The temperature input value.
|
|
time_input (float): Current time in seconds.
|
|
|
|
Returns:
|
|
`true` if tuning is finished, otherwise `false`.
|
|
"""
|
|
now = time_input * 1000
|
|
|
|
if (self._state == PIDAutotuneState.STATE_OFF
|
|
or self._state == PIDAutotuneState.STATE_SUCCEEDED
|
|
or self._state == PIDAutotuneState.STATE_FAILED
|
|
or self._state == PIDAutotuneState.STATE_READY):
|
|
self._state = PIDAutotuneState.STATE_RELAY_STEP_UP
|
|
|
|
self._last_run_timestamp = now
|
|
|
|
# check input and change relay state if necessary
|
|
if (self._state == PIDAutotuneState.STATE_RELAY_STEP_UP
|
|
and input_val > self._setpoint + self._noiseband):
|
|
self._state = PIDAutotuneState.STATE_RELAY_STEP_DOWN
|
|
logging.debug('switched state: {0}'.format(self._state))
|
|
logging.debug('input: {0}'.format(input_val))
|
|
elif (self._state == PIDAutotuneState.STATE_RELAY_STEP_DOWN
|
|
and input_val < self._setpoint - self._noiseband):
|
|
self._state = PIDAutotuneState.STATE_RELAY_STEP_UP
|
|
logging.debug('switched state: {0}'.format(self._state))
|
|
logging.debug('input: {0}'.format(input_val))
|
|
|
|
# set output
|
|
if (self._state == PIDAutotuneState.STATE_RELAY_STEP_UP):
|
|
self._output = self._initial_output - self._outputstep
|
|
elif self._state == PIDAutotuneState.STATE_RELAY_STEP_DOWN:
|
|
self._output = self._initial_output + self._outputstep
|
|
|
|
# respect output limits
|
|
self._output = min(self._output, self._out_max)
|
|
self._output = max(self._output, self._out_min)
|
|
|
|
# identify peaks
|
|
is_max = True
|
|
is_min = True
|
|
|
|
for val in self._inputs:
|
|
is_max = is_max and (input_val >= val)
|
|
is_min = is_min and (input_val <= val)
|
|
|
|
self._inputs.append(input_val)
|
|
|
|
# we don't trust the maxes or mins until the input array is full
|
|
if len(self._inputs) < self._inputs.maxlen:
|
|
return False
|
|
|
|
# increment peak count and record peak time for maxima and minima
|
|
inflection = False
|
|
|
|
# peak types:
|
|
# -1: minimum
|
|
# +1: maximum
|
|
if is_max:
|
|
if self._peak_type == -1:
|
|
inflection = True
|
|
self._peak_type = 1
|
|
elif is_min:
|
|
if self._peak_type == 1:
|
|
inflection = True
|
|
self._peak_type = -1
|
|
|
|
# update peak times and values
|
|
if inflection:
|
|
self._peak_count += 1
|
|
self._peaks.append(input_val)
|
|
self._peak_timestamps.append(now)
|
|
logging.debug('found peak: {0}'.format(input_val))
|
|
logging.debug('peak count: {0}'.format(self._peak_count))
|
|
|
|
# check for convergence of induced oscillation
|
|
# convergence of amplitude assessed on last 4 peaks (1.5 cycles)
|
|
self._induced_amplitude = 0
|
|
|
|
if inflection and (self._peak_count > 4):
|
|
abs_max = self._peaks[-2]
|
|
abs_min = self._peaks[-2]
|
|
for i in range(0, len(self._peaks) - 2):
|
|
self._induced_amplitude += abs(self._peaks[i]
|
|
- self._peaks[i+1])
|
|
abs_max = max(self._peaks[i], abs_max)
|
|
abs_min = min(self._peaks[i], abs_min)
|
|
|
|
self._induced_amplitude /= 6.0
|
|
|
|
# check convergence criterion for amplitude of induced oscillation
|
|
amplitude_dev = ((0.5 * (abs_max - abs_min)
|
|
- self._induced_amplitude)
|
|
/ self._induced_amplitude)
|
|
|
|
logging.debug('amplitude: {0}'.format(self._induced_amplitude))
|
|
logging.debug('amplitude deviation: {0}'.format(amplitude_dev))
|
|
|
|
if amplitude_dev < PIDAutotune.PEAK_AMPLITUDE_TOLERANCE:
|
|
self._state = PIDAutotuneState.STATE_SUCCEEDED
|
|
|
|
# if the autotune has not already converged
|
|
# terminate after 10 cycles
|
|
if self._peak_count >= 20:
|
|
self._output = 0
|
|
self._state = PIDAutotuneState.STATE_FAILED
|
|
return True
|
|
|
|
if self._state == PIDAutotuneState.STATE_SUCCEEDED:
|
|
self._output = 0
|
|
logging.debug('peak finding successful')
|
|
|
|
# calculate ultimate gain
|
|
self._Ku = 4.0 * self._outputstep / \
|
|
(self._induced_amplitude * math.pi)
|
|
print('Ku: {0}'.format(self._Ku))
|
|
|
|
# calculate ultimate period in seconds
|
|
period1 = self._peak_timestamps[3] - self._peak_timestamps[1]
|
|
period2 = self._peak_timestamps[4] - self._peak_timestamps[2]
|
|
self._Pu = 0.5 * (period1 + period2) / 1000.0
|
|
print('Pu: {0}'.format(self._Pu))
|
|
|
|
for rule in self._tuning_rules:
|
|
params = self.get_pid_parameters(rule)
|
|
print('rule: {0}'.format(rule))
|
|
print('Kp: {0}'.format(params.Kp))
|
|
print('Ki: {0}'.format(params.Ki))
|
|
print('Kd: {0}'.format(params.Kd))
|
|
|
|
return True
|
|
return False
|
|
|
|
|
|
async def main():
|
|
"""
|
|
PID AutoTune Tools for Kirdy
|
|
The obtained temperature works best at the target temperature specified.
|
|
Before running PID AutoTune, please
|
|
1. Secure the laser diode onto the LD adapter and copper heat sink
|
|
2. Make sure Kirdy has warmed up and reached thermal equilibrium state
|
|
|
|
In case of PID Autotune Failure, you can
|
|
1. Run the PID Autotune again
|
|
2. Or increase the lookback period
|
|
3. Or increase the sampling rate
|
|
"""
|
|
|
|
# Target temperature of the autotune routine, celsius
|
|
target_temperature = 20
|
|
# Value by which output will be increased/decreased from zero, amps
|
|
output_step = 1
|
|
# Reference period for local minima/maxima, seconds
|
|
lookback = 2.0
|
|
# Determines by how much the input value must
|
|
# overshoot/undershoot the setpoint, celsius
|
|
noiseband = 2.0
|
|
|
|
kirdy = Kirdy()
|
|
kirdy.start_session(host='192.168.1.126', port=1337)
|
|
|
|
await kirdy.wait_until_connected()
|
|
|
|
while not(kirdy.connected()):
|
|
pass
|
|
|
|
await kirdy.laser.set_power_on(False)
|
|
await kirdy.laser.set_i(0)
|
|
|
|
await kirdy.thermostat.set_power_on(False)
|
|
await kirdy.thermostat.set_constant_current_control_mode()
|
|
await kirdy.thermostat.set_tec_i_out(0)
|
|
await kirdy.thermostat.clear_alarm()
|
|
|
|
|
|
signal_handler = SignalHandler()
|
|
signal_handler.setup()
|
|
async def sig_handling():
|
|
await signal_handler.wait_terminate()
|
|
tuner.setFailed()
|
|
asyncio.create_task(sig_handling())
|
|
|
|
await kirdy.device.set_active_report_mode(False)
|
|
|
|
# Configure the Thermistor Parameters
|
|
await kirdy.thermostat.set_sh_beta(3950)
|
|
await kirdy.thermostat.set_sh_r0(10.0 * 1000)
|
|
await kirdy.thermostat.set_sh_t0(25)
|
|
|
|
# Set a large enough temperature range so that it won't trigger overtemperature protection
|
|
await kirdy.thermostat.set_temp_mon_upper_limit(target_temperature + 20)
|
|
await kirdy.thermostat.set_temp_mon_lower_limit(target_temperature - 20)
|
|
|
|
await kirdy.thermostat.set_tec_max_cooling_i(output_step)
|
|
await kirdy.thermostat.set_tec_max_heating_i(output_step)
|
|
|
|
# The Polling Rate of Temperature Adc is equal to the PID Update Interval
|
|
await kirdy.thermostat.config_temp_adc_filter(FilterConfig.Sinc5Sinc1With50hz60HzRejection.f16sps)
|
|
settings = await kirdy.device.get_settings_summary()
|
|
sampling_rate = settings["thermostat"]["temp_adc_settings"]["rate"]
|
|
|
|
print("Settings: {0}".format(settings))
|
|
|
|
tuner = PIDAutotune(target_temperature, output_step,
|
|
lookback, noiseband, 1/sampling_rate)
|
|
|
|
await kirdy.thermostat.set_power_on(True)
|
|
|
|
while True:
|
|
status_report = await kirdy.device.get_status_report()
|
|
|
|
temperature = status_report["thermostat"]["temperature"]
|
|
ts = status_report['ts']
|
|
print("Ts: {0} Current Temperature: {1} degree".format(ts, temperature))
|
|
|
|
if (tuner.run(temperature, ts / 1000.0)):
|
|
print(tuner._state)
|
|
break
|
|
|
|
tuner_out = tuner.output()
|
|
await kirdy.thermostat.set_tec_i_out(float(tuner_out))
|
|
|
|
await kirdy.thermostat.set_tec_i_out(0)
|
|
await kirdy.thermostat.set_power_on(False)
|
|
|
|
if tuner.state() == PIDAutotuneState.STATE_SUCCEEDED:
|
|
pid_params = tuner.get_pid_parameters(tuning_rule="tyreus-luyben")
|
|
await kirdy.thermostat.set_pid_kp(pid_params.Kp)
|
|
await kirdy.thermostat.set_pid_ki(pid_params.Ki)
|
|
await kirdy.thermostat.set_pid_kd(pid_params.Kd)
|
|
await kirdy.thermostat.set_pid_output_max(1.0)
|
|
await kirdy.thermostat.set_pid_output_min(1.0)
|
|
|
|
await kirdy.end_session()
|
|
|
|
if __name__ == "__main__":
|
|
asyncio.run(main())
|