QPFit/sim_dcmotor.m
3bfb25b5e70amaster

sim_dcmotor.m
View Options

			function [X,U,Y,Z] = sim_dcmotor(dat, ctrl)

			xs = dat.xs;
			A = dat.A;
			B = dat.B;
			C = dat.C;
			D = dat.D;
			Ts = dat.Ts;
			[n,m] = size(B);
			Xo = dat.Xo;

			x0 = xs * 0.1;
			Tmax = 20;
			nSim = Tmax / Ts
			t = Ts * [1:nSim-1];
			% yref = -10*heaviside(t-1);
			% yref = piheaviside(sin(t2*pi/15));
			yref = pisin(t2*pi/15);

			% infeas = true;
			% tic
			% while infeas
			% if toc > 2
			% fprintf('Taking a long time to find a feasible initial condition...\n');
			% tic
			% end
			% x0 = (2rand(4,1) - 1) . xBnd;
			% [~, infeas] = gnd.opt({x0, yref(1)});
			% end
			% x0

			X = zeros(n, nSim);
			U = zeros(m, nSim-1);
			Y = zeros(1, nSim-1);
			Z = zeros(n-1, nSim-1);

			X(:,1) = x0;
			for i = 1:nSim-1
			U(:,i) = ctrl(X(:,i), yref(i));
			% if infeas
			% warning('Not feasible %i', i)
			% end
			Y(:,i) = C*X(:,i);
			Z(:,i) = Xo(1:3,:)*X(:,i);
			X(:,i+1) = AX(:,i) + BU(:,i);
			end
			X(:,end) = [];