% springmass.m - calculations on the spring mass system for Ch 2
% RMM, 3 Oct 04

aminit;					% initialize MATALB

% Parameters defining the system
m = 250;				% system mass
k = 40;					% spring constant
b = 60;					% damping constant

% System matrices
A = [0, 1; -k/m, -b/m];
B = [0; 1/m];
C = [1, 0];
sys = ss(A, B, C, 0);

%
% Discrete time simulation
%
% This section explores what happens when we discretize the ODE
% and convert it to a discrete time simulation.
%
Af = 20;				% forcing amplitude
omega = 0.5;				% forcing frequency

% Sinusoidal forcing function
t = [0:0.1:100];
u = Af*sin(omega*t);

% Simulate the system using standard MATLAB routines
[ys, ts] = lsim(sys, u, t);

% Now generate some simulations manually
hvec = [1 0.5 0.1];			% must be multiples of 0.1
clear x;
for iter = 1:length(hvec)
  x(:,1) = [0; 0];  
  h = hvec(iter);
  maxi(iter) = max(t)/h;
  for i = 1:maxi(iter);
    x(:,i+1) = x(:,i) + h*(A*x(:,i) + B*u(h/0.1*i));
    td(i,iter) = (i-1)*h;
    yd(i,iter) = C*x(:,i);
  end
end

% Now plot the results
figure(1); clf; 
plot(t, u);
xlabel('time [sec]');
ylabel('F [N]');

figure(2); clf; subplot(211); hold on;
analh = plot(ts, ys, 'k-', 'LineWidth', AM_data_linewidth);
simh = plot(...
  td(1:maxi(1),1), yd(1:maxi(1),1), 'g+--', ...
  td(1:maxi(2),2), yd(1:maxi(2),2), 'ro--', ...
  td(1:maxi(3),3), yd(1:maxi(3),3), 'b--', ...
  'MarkerSize', 4, 'LineWidth', AM_ref_linewidth ...
);

amaxis([0 50 -2 2]); box on;
xlabel('Time {\itt} [sec]');
ylabel('Position {\itq} [m]');

lgh = legend(gca, [simh; analh], ...
  '{\ith} = 1', '{\ith} = 0.5', '{\ith} = 0.1', 'analytical', ...
  'Location', 'Southwest');
legend(lgh, 'boxoff');

amprint('springmass-euler.eps');

%
% Step response for system ID
%
% In this section we compute a step response and use it to identify
% the parameters of the system.  The plot generated by the step response
% is annotated using xfig, so we don't produce an eps file here.
%
F0 = 20;

% Simulate the system and scale the response
[pos, time] = step(sys, 50);  pos = F0*pos;
figure(3); clf; subplot(211); hold on;
plot(time, pos, 'b', 'LineWidth', AM_data_linewidth);
grid on; box on;

amaxis;
xlabel('Time {\itt} [s]');
ylabel('Position {\itq} [m]');

% Figure out where the features in the plot are
pk1 = 18;
pk2 = 50;

% Add labels to the plot showing the features
line([0 time(pk2)+2], [pos(pk1)+0.001 pos(pk1)+0.001], ...
  'Color', 'k', 'Linewidth', AM_ref_linewidth);
line([0 time(pk2)+2], [pos(pk2)+0.001 pos(pk2)+0.001], ...
  'Color', 'k', 'Linewidth', AM_ref_linewidth);
line([time(pk1) time(pk1)], [0 pos(pk1)+0.05], ...
  'Color', 'k', 'Linewidth', AM_ref_linewidth);
line([time(pk2) time(pk2)], [0 pos(pk2)+0.05], ...
  'Color', 'k', 'Linewidth', AM_ref_linewidth);

amtext(time(pk1)+1, pos(pk1)+0.04, '{\itq}({\itt}_1)');
amtext(time(pk2)+1, pos(pk2)+0.04, '{\itq}({\itt}_2)');
amtext(45, pos(pk2)+0.04, '{\itq}(\infty)');

% annotation('doublearrow', [0.27 0.50], [0.7 0.7]);
arrow([time(pk1)+0.5, 0.275], [time(pk2)-0.5, 0.275], AM_normal_arrowsize);
arrow([time(pk2)-0.5, 0.275], [time(pk1)+0.5, 0.275], AM_normal_arrowsize);
amtext((time(pk1) + time(pk2))/2, 0.2, '{\itT}');

% Redraw the plot so that it's on top
plot(time, pos, 'b', 'LineWidth', AM_data_linewidth);

amprint('springmass-sysid.eps');
