function dX = cartpole_nonlinear(t,X)
% This function simulates the LINEARIZED dynamics of the
% cartpole system. It is written for use with the built-in
% MATLAB ordinary differential equation solver "ode45".
%
% State variables, X, are:
%    X = [x; theta; dx/dt; dtheta;dt]

% For HW1, DO NOT MODIFY THE VALUES BELOW: ---------------
M=1; mp=.5; L=1; g=9.8;        % define system constants
K = [-10.0000   94  -13   27]; % feedback gains (for LQR)
% --------------------------------------------------------
% Definitions of states and control (below) will also
% remain the same for the nonlinear dynamics...
x = X(1);
theta = X(2);
dx = X(3);      % this is cart velocity: dx/dt
dtheta = X(4);  % this is pendulum ang. vel.: dtheta/dt
% ...and you should also keep the control effort (below) unchanged:
Fx = -K(1)*x - K(2)*theta - K(3)*dx - K(4)*dtheta;  % force on cart, Fx
%---------------------------------------------------------


%======== START OF SECTION TO EDIT FOR HW 1 ====================
% BELOW are the EQUATIONS OF MOTION, which you will modify
% when you simulate the nonlinear dynamics.
%
% Linearized equations of motion are:
% 1) d2theta/dt = (M+mp)/M*(g/L)*theta + (1/(M*L))*Fx
d2theta = (M+mp)/M*(g/L)*theta + (1/(M*L))*Fx;
%
% 2) d2x = (mp/M)*g*theta + (1/M)*Fx
d2x = (mp/M)*g*theta + (1/M)*Fx;
%========= END OF SECTION TO EDIT FOR HW 1 =====================

%---------------------------------------------------------------
% Below, we will artificially "saturate" the outputs, so
% MATLAB does not "blow up".  This should only happen if
% the system goes unstable!  
if abs(d2x)>1e4, d2x = abs(d2x)*1e4; end
if abs(d2theta)>1e4, d2theta = abs(d2theta)*1e4; end

% The output of this function is the current state derivative, dX/dt
dX = [dx; dtheta; d2x; d2theta];

% ---------------- end of file -----------------