clear all
% close all
% clc

M=10;    %mass of the cart          [kg]
m=2;     %mass of the pendulum      [kg]
l=1;     %lenght                    [m]
ra=10;   %armature resistance       [ohm]
rr=.1;   %radius of the wheels      [m]
ke=2;    %constant                  [N*m/A]
kc=2;    %constant                  [V*s/rad]
g=9.81;
T = 0.1; %discretization period     [s]

%% Define the matrices of the discretized system

A = [0  1  0      0;
     0 -4 -1.9620 0;
     0  0  0      1;
     0  4 11.7720 0];

B = [0;
     0.2;
     0;
    -0.2];

[Ad,Bd] = c2d(A,B,T);
  
  
%% (a) - Check if it is possible to achieve offset-free tracking 
%        when angular position of the pendulum is measured

C1 = [0 0 1 0];

Sigma1 = [Ad-eye(size(Ad)) Bd; C1 0];
det(Sigma1)                           % = 0 

% Since det(Sigma) = 0, it is not possible to achieve offset-free tracking

%% (b) - Check if it is possible to achieve offset-free tracking 
%        when the position of the cart is measured

C2 = [1 0 0 0];

Sigma2 = [Ad-eye(size(Ad)) Bd; C2 0];
det(Sigma2)                           % ~= 0 

% Since det(Sigma) ~= 0, it is possible to achieve offset-free tracking

% Create the matrices of the augmented dynamics with eta = [x' v]' where
... v(k) is the integration variable with dynamics v(k+1) = v(k) + (y0(k)-
... y(k))

Abar = [Ad zeros(size(Ad,1),1);
       -C2          1         ];
    
Bbar = [Bd;
        0];
    
    
% Check the reachability of the augmented dynamics

rank(ctrb(Abar,Bbar))     % = 5. (Abar,Bbar) is reachable
    
% Design the stabilizing state-feedback controller gain K

P = [exp(-2*T) exp(-2.5*T) exp(-3*T) exp(-4*T) exp(-10*T)];

K = -place(Abar,Bbar,P)

eig(Abar+Bbar*K)  % = [0.3679 0.6703 0.7408 0.7788 0.8187]'
               ...= [exp(-10*T) exp(-4*T) exp(-3*T) exp(-2.5*T) exp(-2*T)]'


%% (c) - Simulation on the Simulink file
% Download pole_placement_integrator_pendulum_anim_sol.slx and run it. You
% can see from this simulation that the integrator action enables
% offset-free tracking for constant reference values. The references that
% are changing with time, however, are not exactly tracked.