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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Problem 6.                                            %
%  Control the pendulum to minimizer the energy          %
%  consumed in driving it to a final position.           %
%                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global mydata  mypenalty

mydata.m = 1;          % m is the mass
mydata.l = 1;          % l is the length of the pendulu
mydata.g = 9.81;       % g is the acceleration of gravit
mydata.c = .5;         % c is the damping constant
mydata.forcedflag = 2; % indicates applied force with control
mydata.linearflag = 0; % indicates the nonlinear model
mydata.forceangle = pi/3; % angle in constant forcing term

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This program finds the optimal closed-loop control     %
% parameters that minimize the total energy consumed     %
% in moving the pendulum from its initial state to the   %
% final specified one within 20 seconds.                 %
% The closed-loop control is                             %
% u = g/l*sin(pi/3)+a*sin(theta)+b*d theta/dt.           %
% E denotes the total energy consumed in this process:   %
%     E \approx h*sum(u(i*h)), for 0<=i<20/h,            %
% where h is length of the time step.                    %
% Note that for the stability of the closed-loop control %
% system we need to impose the constraints               %
% a<g/l and b<c/(ml).                                    %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

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

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Use fminbnd to solve this constrained optimization     %
% problem.                                               %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

mydata.Bc = fminbnd('energy',-10,mydata.c/(mydata.m*mydata.l))

total_energy_consumed=energy(mydata.Bc); 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Use ode45.m to find the numerical solution with        %
% the optimal closed-loop control parameter  b.          %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[t,y]=ode45('pendulum',t,ic);
figure
plot(t,y(:,1))
xlabel('time (t)')
ylabel('\theta (t)')
title('Damped Driven Pendulum with Optimal Control Parameters')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Check the constraints a<g/l and b<c/(ml)               %
% and display the solution.                              %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

disp(sprintf('mypenalty = %f.  If nonzero, optimization ',...
     mypenalty))
disp('did not find a feasible point.')
disp('Damped Driven Pendulum with Optimal Control Parameter')

disp(sprintf('Bc = %f/sec; upper bound = %f', ...
     mydata.Bc, mydata.c/(mydata.m*mydata.l)))

disp(sprintf('Total energy consumed = %f Joules', ...
              total_energy_consumed))