solveCraneODE(x, ptotal, r)

function sol = solveCraneODE(x, ptotal, r)
% solveCraneODE  Solve crane ODE problem
%  SOL = solveCraneODE(X, PTOTAL, R) solves the gantry crane ODE problem.
%     X      : 3-by-1 vector (tp1, tp2, tf)
%     PTOTAL : total displacement of cart
%     R      : height of crane
%
%     X can be a 3-by-n matrix to solve n different conditions.
%
%  SOL is a structure with the following fields:
%     x, ptotal, r, T, Y.
%     T : time vector of the solution to the ODE
%     Y : 2-column matrix of angular position and velocity of the pendulum
%         (in radians)
%
%     If X is a 3-by-n matrix, SOL will be a 1-by-n array of structures.

% Copyright 2011 The MathWorks, Inc.

error(nargchk(3, 3, nargin, 'struct'));

n = size(x, 2);

%% Setup solution structure
sol = struct('x', mat2cell(x, 3, ones(1, n)), ...
   'ptotal', repmat({ptotal}, 1, n), ...
   'r', repmat({r}, 1, n), ...
   'T', cell(1, n), ...
   'Y', cell(1, n));

%%
for id = 1:n
   % Simulate for tf + 10 seconds
   Ttotal = x(3, id) + 10;
   
   % Solving the ODE system using ode45
   [T, Y] = ode45(@odeSystem, [0, Ttotal], ... 
      [0, 0]) ;     % initial conditions
   
   sol(id).T = T;
   sol(id).Y = Y;
end

   function dy = odeSystem(t, y)
      %ODESYSTEM
      %    SWINGEQ = ODESYSTEM(T,Y)
      
      %    This function was generated by the Symbolic Math Toolbox version 5.5.
      %    07-Jan-2011 16:55:34
      
      g = 9.81;
      
      dy(1) = y(2);
      dy(2) = -(accel(t).*cos(y(1))+g.*sin(y(1)))./r;
      
      dy = dy(:); % convert to column vector
      
   end

   function acc = accel(t)
      acc = motionProfile('accel', t, x(:, id), ptotal);
   end

end