%% This is script is accomponying material for the 
% MICRO-450 Basics of robotics for manipulation-Set9-Exercise2.
% Author: Matthieu Gachet
% Date: 22.11.2023

clc
clear all
%% Constant definition

kf = 1     % The motor force constant is 1[N/A]
ks = 0.5   % The leaf spring hinges stifness is 0.5[N/mm]
mg = 50e-3 % the equivalent mass of the flexure hinge guide is 50[g]
ml = 10e-3 % the mass of the load is 10[g]
cf = 0   % viscosity coefficient (null in the problem)

%% Transfer function definition

syms s
s = tf('s'); % Laplace variable definition
Tend = 5 % simulation time
Tstart = -0.1 % Simulation start time


FT = kf/(ks + cf*s + (mg+ml)*s^2); %transfert function definition
[Y, T] = step(FT, Tend); % apply step imput on it



[Ys, Ts] = step(tf(1), Tend); % Step input (for the step graph)

%% step plotting

figure(1)
plot([Tstart; 0; Ts], [0; 0; Ys]) % Start the plot at Tstart and not 0 to 
                                  % clearly see the step
axis([-0.1 Tend -0.5 1.5])
grid minor
xlabel("time[s]")
ylabel("current [A]")
title("Step input")
legend("Current input to the actuator")

%% step response plotting
figure(2)
plot([Tstart; 0; T], [0; 0; Y]) % Start the plot at Tstart and not 0 to 
                                  % clearly see the step
axis([-0.1 Tend min(Y)-0.5 max(Y) + 0.5])
grid minor
xlabel("time[s]")
ylabel("Position [mm]")
title("Step response")
legend("Position of the load")
set(gca, 'YAxisLocation', 'right');