%
% nonlinar dynamics
% inverted pendulum on a cart
%


function xdot = cart2_dynamics(t,x)

global M m b I g l K;

y = x(1);
dy = x(2);
th = x(3);
dth = x(4);

% design a controller
%f = K*x;
f = 0;


% solve for dx/dt
d = [f+ m*l*dth^2*sin(th)- b*dy ;
     -m*g*l*sin(th)	       ];
F = [M+m  	m*l*cos(th);
     m*l*cos(th) (I+m*l^2)];

ddot = F\d;

xdot = [dy; ddot(1); dth; ddot(2)];

