
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                        %
%  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: problem5.m                                 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


global mydata theta10

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.5;        % c is the damping constant
mydata.forcedflag = 1; % indicates applied force
mydata.linearflag = 1; % indicates the linear model
mydata.forceangle = pi/8; % u(t) = m*g*sin(forceangle)
                       %   when forcedflag == 1

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Problem 5.                                            %
%  Solutions to the boundary value problem by            %
%  the shooting method and by finite differences.        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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:10]; % t is a row vector denoting the time instants
            % at which the solutions will be determined.


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                        %
%  Solve the original initial value problem.             %
%                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

theta0 = pi/32;
[t,y]=ode45('pendulum',t,[theta0,0]); 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                       %
%  Solve the boundary value problem.                    %
%                                                       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                       %
%                  Shooting method                      %
%                                                       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                       %
% Apply the shooting method to find the solutions to    %
% the damped, driven pendulum equation on time interval %
% t = [0,10]. The idea behind the shooting method is to %
% guess at the missing initial values, integrate the    %
% equation using your favorite initial value problem    %
% method and then use the results to improve the guess. %
% Hence, a shooting method involves using a nonlinear   %
% equation solver with function evaluation through an   %
% initial value problem ODE solver. We'll use the       %
% function fzero as the nonlinear equation solver and   %
% ode45 as the ODE solver for the shooting method.      %
% We start with the initial guess d theta /dt (0) = 0.5 %
%                                                       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% We solve the problem with theta(0) = theta0 and       %
% theta(10) = whatever value our previous solution      %
% predicted.                                            %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

theta10 = y(end,1); 
t = [0:h:10];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Use fzero with knowledge of theta(0) and theta(10) to %
% find the missing initial condition d theta(t)(0)/dt.  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

thetaprime0 = fzero('shooting',1); 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Use the initial condition to reconstruct the solution %
% to compare with the previous one.                     %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[t,ys]=ode45('pendulum',t,[theta0 thetaprime0 ]); 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                        %
%                  Finite Difference Method              %
%                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                        %
% Now use the finite difference method to solve this     %
% boundary value problem.                                %
% Replace all first derivatives                          %
%         y'(t) by (y(t+h)-y(t-h))/(2*h)                 %
% and second derivatives                                 %
%         y''(t) by (y(t+h) -2*y(t)+y(t-h))/(h^2).       %
% Let h = ts /n for a large number n                     %
% and define y_j = y(jh), j = 0,...,n.                   %
% Then, the boundary conditions can be                   %
% stated as y_0 = 0, y_{n+1} = thetaB.                   %
% Transform the second order differential  equation      %
%      m*l*theta"(t)+c*x'(t)+m*g*theta(t) = u(t)         %
% to a system of n-1 linear equations with n-1 unknowns. %
% Use LU factorization to solve the resulting linear     %
% system of equations.                                   %
%                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

h=0.01; 
n=10/h; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                        %
% The linear system is A (yd) = b   where                %
% yd is the vector of approximate values of y and        %
% all elements of A are zero except                      %
% the subdiagonal elements                               %
%    c1 = (m*l/h^2)  -  c/(2h)                           %
% the main diagonal elements                             %
%    c2 = (-2*m*l/h^2) + m*g,                            %
% and the superdiagonal elements                         %
%    c3 = (m*l/h^2)  +  c/(2h)                           %
% and                                                    %
% b(i) = u                                               %
%    except that the first and last entries have the     %
%    extra terms                                         %
%   -c1 y(0)   and                                       %
%   -c3 y(n)  respectively.                              %
% y(i) is the approximation to theta(ih).                %
%                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

c1 = mydata.m*mydata.l/(h*h) - mydata.c/(2*h);
c2 = -2*mydata.m*mydata.l/(h*h) + mydata.m*mydata.g;
c3 = mydata.m*mydata.l/(h*h) + mydata.c/(2*h);

A = toeplitz([c2,c1,zeros(1,n-3)], [c2,c3,zeros(1,n-3)] );
u = mydata.m * mydata.g * sin(mydata.forceangle);
b = u * ones(n-1,1);
b(1)   = b(1)   -c1*theta0;
b(n-1) = b(n-1) -c3*theta10;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Use LU factorization to solve the                      %
% linear system of equations.                            %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

yd = A\b; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Insert boundary values theta(0) and theta(10)          %
% to form the solution vector.                           %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

yd=[theta0; yd; theta10];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot the three solutions for comparison.               %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure(3)
subplot(2,1,1)
plot(t,y(:,1),t,ys(:,1),t,yd)
xlabel('time (t)')
ylabel('\theta (t)')
legend('Original solution','Shooting solution', ...
       'Finite difference solution')
title('Solutions to the Boundary Value Problem')
subplot(2,1,2)
plot(t,y(:,1)-ys(:,1),t,y(:,1)-yd)
xlabel('time (t)')
ylabel('\theta (t)')
legend('Original - Shooting', ...
       'Original - Finite Difference')
