function pdot = rotmass(t,p)

%

% Rotating mass example

% c1 - muliplies displacement error to apply torque

%    proportion gain; any sping would be added to this

% c2 - multiplies velocity to get a resisting torque

%      this term contains the derivative gain

%      you would add back enf to this but remember to have 

%        the correct units

% c3 - constant torque applied to system

% c4 - integral gain

% Ieq - equivalent rotary inertia

% thsp - displacement setpoint

% fr - friction magnitude

%

% p(1) - displacement (rad)

% p(2) - velocity (rad/s)

% p(3) - integral error (rad*s)

%

c1 = 2; c2 = 1.4; Ieq = 0.5; c3 = 0.2; thsp = 0.4;

fr = 0.0; c4 = 1;

if (p(2) > 0)

    f=-fr;

else

    f=fr;

end

%

pdot = zeros(size(p));

%

% define the 3 differential equations

%

pdot(1) = p(2);

pdot(2) = (c1*(thsp-p(1))+c3-c2*p(2)+f+c4*p(3))/Ieq;

pdot(3) = thsp-p(1)

%

% put in an anti wind up limit

%

if(abs(thsp-p(1)) > (2*c3/c1))

    pdot(3) = 0;

end

% eof - rotmass.m

%

%the following are entered in the command window

%

% define initial conditions (in this case all zer0)

%

%p0 = [0 0 0];

%

% execute  solver from 0 to 6.3s

%

%[t,p] = ode45('rotmass',[0 6.3],p0);

%

% plot the 3 state varialbles

%

%plot(t,p(:,1),t,p(:,2),t,p(:,3));