% Control3_2
% Problem 2, Comuptación I, Control 3. 2017/04/21
% J.M.Soler

% Set parameters
clear all;  % clean variables
R=0.85;     % trajectory radius of mass A (m)
T=4.5;      % rotation period of mass A (s)
mB=0.9;     % mass B (kg)
k=8.8;      % elastic constant between A and B (N/m)
g=9.8;      % gravitational acceleration (m/s^2)
dt=1.e-3;   % time step (s)
tmax=2*T;   % simulation time (s)
rB0=[R;-R]; % initial position of B (m)
vB0=[0;0];  % initial velocity of B (m/s)

% Define position and force functions
wA=2*pi/T;                             % angular frequency of mass A
posA=@(t) R*[cos(wA*t);sin(wA*t)];     % position of mass A
forceB=@(rA,rB) -k*(rB-rA)+[0;-mB*g];  % force on B
myForce=@(rB,vB,t) forceB(posA(t),rB); % forceB, as required by rungeKutta

% Find trajectories
t=[0:dt:tmax];
rA=posA(t);
rB=rungeKutta(myForce,mB,rB0,vB0,dt,tmax);
rB=squeeze(rB);

% Plot trajectories
plot(rA(1,:),rA(2,:),rB(1,:),rB(2,:));
legend('A','B');
axis equal;
axis([-1.5,1.5,-2.5,1]);
grid on;
xlabel('x (m)');
ylabel('y (m)');
