Trying to find u value where initial conditions are theta1=theta2=theta1dot=theta2dot=0 and end at theta1=pi and theta2=theta1dot=theta2dot=0.
u is found by equation
clear; clc;
% Initalize variables
syms theta1(t) theta2(t) r1 r2 m1 m2 g u
% Initalize kinematic constraints
x1 = r1*sin(theta1);
y1 = -r1*cos(theta1);
x2 = x1+r2*sin(theta2+theta1);
y2 = y1-r2*cos(theta2+theta1);
% Initalize velocity equations
x1dot=diff(x1);
y1dot=diff(y1);
x2dot=diff(x2);
y2dot=diff(y2);
% Solve for potential energy
pe=m1*g*y1+m2*g*y2;
%Solve for kinetic energy
ke=1/2*m1*(x1dot^2+y1dot^2)+1/2*m2*(x2dot^2+y2dot^2);
simplify(ke);
% Solve for L=KE-PE
l=ke-pe;
simplify(l);
% Initalize variables
syms theta1dot theta2dot
% Solve for partial derivitive of L by theta1dot
eqn1=subs(diff(subs(l(t), diff(theta1(t)), theta1dot), theta1dot), theta1dot, diff(theta1(t)));
eqn1=simplify(eqn1);
% Solve for the time derivative of eqn1
eqn1=diff(eqn1);
eqn1=simplify(eqn1);
% Solve for partial derivitive of L by theta1
a=simplify(diff(l,theta1));
% Solve for first equation of motion
eqn1=simplify(eqn1-a)-u
% Solve for partial derivitive of L by theta2dot
eqn2=subs(diff(subs(l(t), diff(theta2(t)), theta2dot), theta2dot), theta2dot, diff(theta2(t)));
eqn2=simplify(eqn2);
% Solve for the time derivative of eqn2
eqn2=diff(eqn2);
eqn2=simplify(eqn2);
% Solve for partial derivitive of L by theta2
z=simplify(diff(l,theta2));
% Solve for second equation of motion
eqn2=simplify(eqn2-z)
% Initalize values for variables
r1 = 1;
r2 = 1;
m1 = 2.5;
m2 = 2.5;
g = 9.81;
u = 37;
[V,S] = odeToVectorField(subs(eqn1),subs(eqn2))
M = matlabFunction(V,'vars',{'t','Y'})
% Initalize initial condition values
% T_1=pi/2;
% T_2=round((9+8+8+2)/4);
% T_1_dot=0.005;
% T_2_dot=0;
% T_1=pi;
% T_2=pi/6;
% T_1_dot=0;
% T_2_dot=round((6+9+8+2)/4)/-100;
% Initalize initial condition values
T_1=0;
T_2=0;
T_1_dot=0;
T_2_dot=0;
initCond = [T_2 T_2_dot T_1 T_1_dot];
% Solve for theta values in ODE
sols = ode45(M,[0 30],initCond);
% Plot theta values of ODE
plot(sols.x,sols.y)
legend('\theta_2','d\theta_2/dt','\theta_1','d\theta_1/dt')
title('Solutions of State Variables')
xlabel('Time (s)')
ylabel('Solutions (rad or rad/s)')
% Initalize constraint equations
x_1 = @(t) r1*sin(deval(sols,t,3));
y_1 = @(t) -r1*cos(deval(sols,t,3));
x_2 = @(t) r1*sin(deval(sols,t,3))+r2*sin(deval(sols,t,1)+deval(sols,t,3));
y_2 = @(t) -r1*cos(deval(sols,t,3))-r2*cos(deval(sols,t,1)+deval(sols,t,3));
% Animate pendulum
fanimator(@(t) plot(x_1(t),y_1(t),'bo','MarkerSize',m1*10,'MarkerFaceColor','b'));
axis equal;
hold on;
fanimator(@(t) plot([0 x_1(t)],[0 y_1(t)],'b-'));
fanimator(@(t) plot(x_2(t),y_2(t),'co','MarkerSize',m2*10,'MarkerFaceColor','c'));
fanimator(@(t) plot([x_1(t) x_2(t)],[y_1(t) y_2(t)],'c-'));
fanimator(@(t) text(-0.3,0.3,"Timer: "+num2str(t,2)));
hold off;
% Play animation
playAnimation(gcf,'AnimationRange',[0 10],'FrameRate',15)
