%
% simd.m  -  This script calls the "update" function to simulate the
% dynamic behavior of the three link manipulator.

RAD = pi/180;   % Conversion factors
DEG = 180/pi;

theta = input('Enter initial joint angles in degrees like [t1 t2 t3]: ');
tf = input('Enter final time in seconds: ');
period = input('Enter display period in seconds (0.02 works well): ');

theta = theta'*RAD;     % Convert to column vector in radians

thetad = zeros(3,1);    % Start with zero velocity

tau = [0 0 0]'; % Joint torques are all zero for this exercise.

pos = [];       % Null matrix to hold joint position results
vel = [];       % Null matrix to hold joint velocity results

% Main loop to compute motion using the update() function

for t = 0:period:tf
    pos = [pos;theta'];
    vel = [vel;thetad'];
    [theta,thetad] = update(theta,thetad,tau,period);
end;

% The rest of the code displays a stick-figure animation of the robot.

t = [0:period:tf]';

N = length(pos);    % Find number of samples
x = zeros(N,4);     % Arrays to hold vectors
y = zeros(N,4);     % of manipulator endpoints

for i = 1:N     % Precompute vectors of endpoints
    [x(i,:),y(i,:)] = draw_robot(pos(i,:));
end;

scrz = get(0,'screensize');
hf = figure('position',[1 scrz(4)*0.75 scrz(3)*0.7 scrz(4)*0.75]);
axis([-1.5 1.5 -1.5 1.0]);   % Axis limits
ha = gca;   % Get handle to axes
set(ha,'fontsize',24);
xlabel('X POSITION (M)');
ylabel('Y POSITION (M)');
grid;
hold on;

p = plot(x(1,:),y(1,:),'linewidth',3,'color','k');  % Plot initial position
ht = text(-1.25,0.755,'Press RETURN to start...,');
set(ht,'fontsize',24);
pause;

for i = 1:N
    set(p,'xdata',x(i,:),'ydata',y(i,:));
    drawnow;
end;
ht = text(-1.25,0.555,'Motion Completed,');
set(ht,'fontsize',24);
ht = text(-1.25,0.355,'Press RETURN to quit.');
set(ht,'fontsize',24);pause;
delete(hf);