clear all
close all

t=linspace(0,10);

theta1=26.565-3.49695*t.^2 +0.23313*t.^3;

d2=sqrt(5)+0.02292*t.^2 -1.528*10^(-3)*t.^3;

theta3=-90+9*t;

figure(1)
subplot(3,1,1)
plot(t,theta1)
grid on
xlabel('t')
ylabel('theta1')
subplot(3,1,2)
plot(t,d2)
grid on
xlabel('t')
ylabel('d2')
subplot(3,1,3)
plot(t,theta3)
grid on
xlabel('t')
ylabel('theta3')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

theta1=(26.565-3.49695*t.^2 +0.23313*t.^3)*(pi/180);

d2=sqrt(5)+0.02292*t.^2 -1.528*10^(-3)*t.^3;


x0=d2.*cos(theta1)
y0=d2.*sin(theta1)

figure(2)
plot(y0,x0)
xlabel('y0')
ylabel('x0')


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all

t=linspace(0,5);
t2=linspace(5,10);
t3=linspace(0,10);
theta3=-90+9*t3;

thetaf=-26.565;
theta0=26.565;

a01=26.565;
a11=0;
a21=(3/5^2)*(thetaf-theta0) -(1/5)*(-10.62);
a31=(-2/5^3)*(thetaf-theta0) +(1/5^2)*(-10.62);

a02=atand(2)-90;
a12=-10.62;
a22=(3/5^2)*(-90-thetaf)-(2/5)*(-10.62);
a32=(-2/5^3)*(-90-thetaf)+(1/5^2)*(-10.62);

theta11=a01+a21*t.^2 +a31*t.^3;
d21=sqrt(5);
theta12=a02+a12*(t)+a22*(t).^2 +a32*(t).^3;
d22=sqrt(5)+0.0917*(t).^2 -0.0122*(t).^3;

figure(3)
subplot(3,1,1)
plot(t,theta11)
hold on
plot(t2,theta12)
grid on
xlabel('t')
ylabel('theta1')
subplot(3,1,2)
plot(t,d21);
hold on
plot(t2,d22)
grid on
xlabel('t')
ylabel('d2')
subplot(3,1,3)
plot(t3,theta3)
grid on
xlabel('t')
ylabel('theta3')


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

ad0=sqrt(5);
ad1=0;
ad2=(3/5^2)*(sqrt(9)-sqrt(5));
ad3=(-2/5^3)*(sqrt(9)-sqrt(5));
d22=sqrt(5)+ad2*(t2-5).^2 +ad3*(t2-5).^3;

x01=d21.*cosd(theta11);
y01=d21.*sind(theta11);
x02=d22.*cosd(theta12);
y02=d22.*sind(theta12);

figure(4)
plot(y01,x01)
hold on
plot(y02,x02)
xlabel('y0')
ylabel('x0')
hold off

figure(5)
plot(theta12)