%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                        %
%  A solution to CSE For Your Homework Project 2         %
%  Robot Arm Control: Like a Pendulum Swings             %
%  Dianne P. O'Leary and Yalin E. Sadguyu                %
%  12/02 and 01/03                                       %
%                                                        %
%  File name: problem4.m                                 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


global mydata theta10

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The data in mydata specifies the properties of the     %
% pendulum, whether there is damping or applied force,   %
% and whether the model is linear or nonlinear.          %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

mydata.m = 1           % m is the mass
mydata.l = 1;          % l is the length of the pendulum
mydata.g = 9.81;       % g is the acceleration of gravity
mydata.c = 0;          % c is the damping constant
mydata.forcedflag = 0; % indicates no applied force
mydata.linearflag = 0; % indicates the nonlinear model
mydata.forceangle = 0; % u(t) = m*g*sin(forceangle)
                       %   when forcedflag == 1


angles = [pi/8,pi/4,pi/3] % candidates for forceangle

h=0.01; % h is the time step for which the solution y(i*h) 
        % will be determined where i is a positive integer.

t=[0:h:20]; % t is a row vector denoting the time instants
            % at which the solutions will be determined.

ic=[pi/4 0]; % ic is a row vector of initial conditions
             % theta(0) and theta'(0).

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Problem 4.                                            %
%  Solutions to various initial value problems for the   %
%  pendulum equation.                                    %
%                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% First, the nonlinear undamped, undriven model          %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[t,ynuu] = ode45('pendulum',t,ic); 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Then the linear undamped, undriven model.              %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

mydata.linearflag = 1;
[t,yluu] = ode45('pendulum',t,ic);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Next, the linear damped, undriven model.               %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

mydata.c = .5;
[t,yldu] = ode45('pendulum',t,ic); 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Then the nonlinear damped, undriven model.             %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

mydata.linearflag = 0;
[t,yndu] = ode45('pendulum',t,ic);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Now plot the four solutions.                           %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure(1)
subplot(2,1,1)
plot(t,ynuu(:,1),'r-',t,yluu(:,1),'b','LineWidth',1.5)
legend('Nonlinear model','Linear model')
xlabel('time (t)')
ylabel('\theta (t)')
title('Undamped Undriven Pendulum')
subplot(2,1,2)
plot(t,yndu(:,1),'r-',t,yldu(:,1),'b','LineWidth',1.5)
legend('Nonlinear model','Linear model')
xlabel('time (t)')
ylabel('\theta (t)')
title('Damped Undriven Pendulum')
print -depsc pendresults.eps

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Finally, the damped, driven models.                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure(2)

for k=1:3,
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  The nonlinear damped, driven model.                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   mydata.linearflag = 0;
   mydata.forcedflag = 1;
   mydata.forceangle = angles(k);
   [t,yndf] = ode45('pendulum',t,ic); 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  The linear damped, driven model.                      %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   mydata.linearflag = 1;
   [t,yldf] = ode45('pendulum',t,ic);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  The plot                                              %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   subplot(3,1,k)
   plot(t,yndf(:,1),t,yldf(:,1))
   legend('Nonlinear model','Linear model')
   xlabel('time (t)')
   ylabel('\theta (t)')
   gg = axis;
   ypos = gg(3) + .1*(gg(4)-gg(3));
   text(7,ypos,sprintf('Damped Driven Pendulum, \\theta_f=%f',angles(k)));

end