%
% File: invpendcinit.m
% Initialization of controller for inverted pendulum simulation
% Created by Marc Bodson. Last revised: 05/23/25
%
% Controller design
%
% TO BE FILLED
% 
% Discrete-time implementation and controller initialization
%
sysc=tf(numc,denc);[ac,bc,cc,dc]=ssdata(c2d(sysc,dt,'zoh'));
syscf=tf(numcf,dencf);[af,bf,cf,df]=ssdata(c2d(syscf,dt,'zoh'));
xc=zeros(length(bc),1);xf=zeros(length(bf),1);
%
% Set-up of the simulation and storage variables
%
npoints=round(tmax/dt);       % Number of simulation points
ts=zeros(npoints,1);          % Time variable
xcoms=zeros(npoints,1);       % Position command
xrefs=zeros(npoints,1);       % Position reference
ths=zeros(npoints,1);         % Beam angle
xcarts=zeros(npoints,1);      % Cart position
ip=1;                         % Initial index for storage