HendiControlFirmware/matlab/brew_eval.m

## Copyright (C) 2018 Jens Ahrensfeld
## 
## This program is free software; you can redistribute it and/or modify it
## under the terms of the GNU General Public License as published by
## the Free Software Foundation; either version 3 of the License, or
## (at your option) any later version.
## 
## This program is distributed in the hope that it will be useful,
## but WITHOUT ANY WARRANTY; without even the implied warranty of
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
## GNU General Public License for more details.
## 
## You should have received a copy of the GNU General Public License
## along with this program.  If not, see <http://www.gnu.org/licenses/>.

## Author: Jens Ahrensfeld <ahrensfeld@w2ess001vm>
## Created: 2018-02-08

function brew_eval(varargin)
%
% brew_eval('M', 20.0, 'cont_ctrl', 0, 'kp', 0.1, 'ki', 0.1, 'kd', 100, 'T', 1800, 'mass_kleak', 0.2, 'P_q', 100, 'mass_delay', 100, [50 60 70 80]);

WITH_LOWPASS = 1;
WITH_RUNNING_MEAN = 0;

params = struct ('cont_ctrl',0, 'C',4.19e3, 'M',10, 'mass_kleak',0.1, 'mass_delay',50, 'P_min',0, 'P_max',3000, 'P_q',100, 'T',100, 'dt',1.0, 'Td',10.0, 'theta_lock',0.5, 'theta_amb',20.0, 'kp',10, 'ki',0.04, 'kd',500, 'pid_kleak',0.1, 'k_noise', 0.01, 'k_noise2', 0.0, 'f_noise2', 0.01);
units = struct ('cont_ctrl', '', 'C', 'J/(kg*K)', 'M', 'kg', 'mass_kleak', '', 'mass_delay', 's', 'P_min', 'W', 'P_max', 'W', 'P_q','W', 'T', 's', 'dt', 's', 'Td', 's', 'theta_lock', '°C', 'theta_amb', '°C', 'kp', '', 'ki', '', 'kd', '', 'pid_kleak', '', 'k_noise', '', 'k_noise2', '', 'f_noise2', 'Hz');

% Parse parameters
names = fieldnames(params);
varargin
for k=1:nargin-1,
	for n=1:length(names),
		if strcmpi(varargin{k}, names{n})
			params.(names{n}) = varargin{k+1};
		end
	end
end
	
print_parameters(params)

s_mass = 0;
s_pid = [];
s_pid_rate = [];
theta_ist = params.theta_amb;
theta_last = params.theta_amb;
thetas = varargin{nargin}
heatRate = 0;
heatRateMeasureInterval = 30.0; % seconds
heatRateMeasureCount = 0; % seconds
n = 0;
nSteps = params.P_max / params.P_q;

N = 2*fix(length(thetas)*params.T/params.dt);

lp_N = 17;
[lp_b, lp_a] = butter (lp_N-1, 0.125);
lp_z = zeros(1, length(lp_b)-1);
maz = 17;

for k=1:length(thetas)
	theta_soll = thetas(k);
	hold_theta = 0;
	temp_ok = 0;
	while n < N,
		err = theta_ist - theta_soll;
		[y, yc, s_pid] = pid(s_pid, params.kp, params.ki*params.dt, params.kd*params.dt, 1-params.pid_kleak, -err);

		% Rate controller
		err_rate = heatRate - 1.0;
		[y_rate, yc_rate, s_pid_rate] = pid(s_pid_rate, 1.0*params.dt, 0.001*params.dt, 0*params.dt, 1-params.pid_kleak, -err_rate);
		P_rate = max(params.P_min+1, min(params.P_max, y_rate));
		% Rate controller

		P_norm = max(params.P_min/params.P_max, min(1.0, yc));
		P_cont = P_norm * params.P_max;
		P_cont_q = fix(P_norm * nSteps)*params.P_q;
		if params.cont_ctrl
			P = P_cont_q;
		else
			duty = mod(n*params.dt/params.Td*params.P_max, params.P_max);
			heat = (duty < P_cont_q);
			P = params.P_max * heat;
		end
		[theta, s_mass] = mass(s_mass, params.dt, params.C, params.M, params.mass_kleak, P, params.mass_delay, theta_ist-params.theta_amb);
		theta_ist = params.theta_amb + theta + params.k_noise*randn()/sqrt(12) + params.k_noise2*cos(params.f_noise2*2*pi*n*params.dt);
		
		if WITH_RUNNING_MEAN
			[theta_ist, maz] = movingavg(theta_ist, maz);
		end
		if WITH_LOWPASS
			[theta_ist, lp_z] = filter(lp_b, lp_a, theta_ist, lp_z);
		end
		if heatRateMeasureCount <= 0
			heatRate = 60*(theta_ist-theta_last)/params.dt/heatRateMeasureInterval;
			heatRateMeasureCount = heatRateMeasureInterval;
			theta_last = theta_ist ;
		else
			heatRateMeasureCount = heatRateMeasureCount - params.dt;
		end	
		n = n + 1;
		theta_ist = theta_ist;
		heatRate_(n) = heatRate;
		theta_(n) = theta_ist;
		err_(n) = err;
		p_(n) = P;
		pc_(n) = P_cont;
		pcq_(n) = P_cont_q;
		y_(n) = y;
		P_rate_(n) = y_rate;
		if hold_theta > 0
			hold_theta = hold_theta - 1;
			if hold_theta == 0
				disp('Temperature next')
				break;
			end
		elseif abs(err) < params.theta_lock
			hold_theta = params.T/params.dt;
			temp_ok = 1;
			disp('Temperature is OK!')
		end
	end
end
close all;
t = (0:n-1)*params.dt;

subplot(3,1,1)
plot(t, theta_); grid; xlabel('t/s'); ylabel('Theta/°'); title('Temperature');
subplot(3,1,2)
plot(t, p_, 'b-', t, y_, 'g-', pc_, 'r-'); grid; xlabel('t/s'); ylabel('P/W'); title('Power Control'); legend('P','y','P_{cont}');
subplot(3,1,3)
plot(t, err_); grid; xlabel('t/s'); ylabel('err/K'); title('Error'); legend('Error');

figure;
subplot(3,1,1)
plot(t, heatRate_); grid; xlabel('t/s'); ylabel('Theta/°'); title('Aufheizrate'); axis ([0 length(t)-1 0 5])
subplot(3,1,2)
plot(t, P_rate_); grid; xlabel('t/s'); ylabel('Theta/°'); title('Aufheizrate');
subplot(3,1,3)
plot(t, heatRate_); grid; xlabel('t/s'); ylabel('Theta/°'); title('Aufheizrate');

function [theta, s] = mass(si, dt, C, M, L, P, Td, dTheta)
s = si;
if ~isstruct(si)
	alpha = 1.0;
	if Td > 0
		alpha = dt/Td;
	end
	s = struct('e', 0, 'a', alpha, 'x', 0);
end

s.e = s.e * (1-((L*dTheta)*dt)/(M*C));
s.x = (1-s.a)*s.x + s.a*P*dt;
s.e = s.e + s.x;
theta = s.e/(M*C);
endfunction

function [y, y_clamped, s] = pid(si, kp, ki, kd, kleak, x)
s = si;
if ~isstruct(si)
	s = struct('p', 0, 'i', 0, 'd', 0, 'x', 0, 'y_min', -0.0, 'y_max', 1.0, 'diff_aw', 0.0);
end

s.d = kd*(x - s.x);
s.i = kleak*s.i + ki*x + s.diff_aw;
s.p = kp*x;
s.x = x;

y = s.p + s.i + s.d;
y_clamped = max(s.y_min, min(s.y_max, y));

if (y < s.y_min)
	s.diff_aw = abs(y - s.y_min);
end

if (y > s.y_max)
	s.diff_aw = -abs(y - s.y_max);
end


endfunction

function print_parameters(params)
align = 32;
names = fieldnames(params);
for n=1:length(names),
	fprintf('%s', names{n});
	remain = align - length(names{n});
	if remain < 0
		error('Invalid variable length!');
	else
		for j=1:remain
			fprintf(' ');
		end
		fprintf('= %g %s\n', params.(names{n}), units.(names{n}));
	end
end
endfunction

endfunction