%
% chpt6.m  -  This script calls the "update" function to simulate the
% dynamic behavior of the three link manipulator for the Craig 
% Programming Exercises (PART 6).
%

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: ');

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

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

tau = zeros(3,1);       % Joint torques are zero

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

for t = 0:period:tf
    pos = [pos;theta'*DEG];     % Save joint angles in degrees
    vel = [vel;thetad'*DEG];    % Save joint velocities in deg/s
    [theta,thetad] = update(theta,thetad,tau,period);
end;

t = [0:period:tf]';

disp(' ');
disp('Joint positions (deg) are in array "pos" ');
disp('Joint velocities (deg/s) are in array "vel" ');
disp('Time vector in array "t" ');