function aux = pendulum(x1,thetadesired,Delta,N,K1,K2);

%%  Jorge Cortes and William Dunbar

%%  Applied Mathematics and Statistics
%%  University of California at Santa Cruz
%%  jcortes@soe.ucsc.edu

%%  Computer Engineering
%%  University of California at Santa Cruz
%%  dunbar@soe.ucsc.edu
	
%%  Copyright (c) July 2005, William Dunbar, Jorge Cortes
%%  Permission is granted to copy, modify and redistribute this file, 
%%  provided that this header message is retained.

% Synthax is pendulum(x1,thetadesired,Delta,N,K1,K2)
% x1 = [Theta1,Theta0] is starting point
% thetadesired is desired equilibrium
% K1, K2 are control gains
% Delta is time step
% N is number of iterations

% udesired is control that makes xdesired an equilibrium
udesired = sin(thetadesired);

% aux is sequence of angle positions
aux(1) = x1(2);
aux(2) = x1(1);

for i=2:N-1
    aux(i+1) = 2*aux(i) - aux(i-1) + Delta^2 *(udesired - sin(aux(i)) + K1*(thetadesired - aux(i)) + K2*(thetadesired - aux(i-1)));
end

elapsedtime = [1:1:N]*Delta;
plot(elapsedtime,aux)
xlabel('Elapsed time')
ylabel('Angular position')
title('Pendulum example with feedback control')
