%% We reset all variables and clean the mess
clc
clear
close all

% Simulation parametres:
Tsim=1e-3; % what is the time step required to perform the simulation (1 ms)
Tcontrol=1e-2; % what is the sampling period of the speed controller (10 ms)
Ncontrol=Tcontrol/Tsim;
tmax_sim=7; % what is the time horizon of the simulation (100 s)
SimSteps=tmax_sim/Tsim;
ControlSteps=floor(tmax_sim/Tcontrol);

% In this demonstration we test the response of the limiting mechanism when
% a step change of the speed reference is requested. The following
% parameter set the amplitude of the step:
TestAmplitude=1;

% Parameters rate limiter plus low pass filter
% Maximum acceleration allowed:
limder=0.1*9.81*Tcontrol; % this corresponds to 10% of the gravity (1 m/s^2)
% Time constant of the low pass filter
tau=1; % 1s for a maximum acceleration of 1 m/s^2 gives a jerk of 1 m/s^3
K1=(Tcontrol+2*tau)/Tcontrol;
K2=(Tcontrol-2*tau)/Tcontrol;

% The following lines initialise the variables required for the simulation
x1=0;
vrefp=0;
vrefrate=0;

tab_t=zeros(1,SimSteps);
tab_vref=zeros(1,SimSteps);
tab_vrefp=zeros(1,SimSteps);
tab_vrefrate=zeros(1,SimSteps);

% the following code simulates the operation of the limiting mechanism
m=1;
for t=0:Tsim:tmax_sim;
    if(mod(t,Tcontrol)==0)
        vref=(t>0.2)*TestAmplitude;
        vrefrate=min(max(vref-x1,-limder),limder)+x1;
        x1n=vrefrate;
        vrefp=(x1n+x1-K2*vrefp)/K1;
        x1=x1n;
    end
    tab_t(m)=t;
    tab_vref(m)=vref;
    tab_vrefp(m)=vrefp;
    tab_vrefrate(m)=vrefrate;
    
    m=m+1;
end

%% Next we produce a few plots to show the performance of the limiting mechanism

p=figure('Position',[100,100,400,500]);
subplot(3,1,1);
plot(tab_t,tab_vref,'r');
grid on
hold on
plot(tab_t,tab_vrefrate,'k');
plot(tab_t,tab_vrefp,'b');
axis([0 tmax_sim -TestAmplitude*0.1 TestAmplitude*1.1]);
legend('v_{ref}','v_{ref}^{rate}','hat v_{ref}');
ylabel('v [m/s]');

avref=(tab_vref((Ncontrol+1):Ncontrol:(SimSteps))-tab_vref(1:Ncontrol:...
    (SimSteps-Ncontrol)))/(Tsim*Ncontrol);
avrefrate=(tab_vrefrate((Ncontrol+1):Ncontrol:(SimSteps))...
    -tab_vrefrate(1:Ncontrol:(SimSteps-Ncontrol)))/(Tsim*Ncontrol);
avrefp=(tab_vrefp((Ncontrol+1):Ncontrol:(SimSteps))...
    -tab_vrefp(1:Ncontrol:(SimSteps-Ncontrol)))/(Tsim*Ncontrol);

subplot(3,1,2);
plot(tab_t(1:Ncontrol:(SimSteps-Ncontrol)),avref,'r');
grid on
hold on
plot(tab_t(1:Ncontrol:(SimSteps-Ncontrol)),avrefrate,'k');
plot(tab_t(1:Ncontrol:(SimSteps-Ncontrol)),avrefp,'b');
legend('a_{ref}','a_{ref}^{rate}','hat a_{ref}');
axis([0 tmax_sim -0.1 1.5]);
ylabel('a [m/s^2]');

jvref=(avref(2:(ControlSteps-1))-avref(1:(ControlSteps-2)))/(Tsim*Ncontrol);
jvrefrate=(avrefrate(2:(ControlSteps-1))-avrefrate(1:(ControlSteps-2)))...
    /(Tsim*Ncontrol);
jvrefp=(avrefp(2:(ControlSteps-1))-avrefp(1:(ControlSteps-2)))/(Tsim*Ncontrol);

subplot(3,1,3);
plot(tab_t(1:Ncontrol:(SimSteps-2*Ncontrol)),jvref,'r');
grid on
hold on
plot(tab_t(1:Ncontrol:(SimSteps-2*Ncontrol)),jvrefrate,'k');
plot(tab_t(1:Ncontrol:(SimSteps-2*Ncontrol)),jvrefp,'b');
legend('j_{ref}','j_{ref}^{rate}','hat j_{ref}');
axis([0 tmax_sim -1.2 1.2]);
ylabel('j [m/s^3]');
xlabel('t [s]');