t1=0:100:3500;
t2=3500:100:7000;
t3=7000:100:10500;
t4=10500:100:14000;
t5=14000:100:17500;
t6=17500:100:21000;
t7=21000:100:22200;
t8=22200:100:25700;
t9=25700:100:29200;
t10=29200:100:32700;
t11=32700:100:36200;
t12=36200:100:39700;
t13=39700:100:43200;
a1=3500;
a2=7000;
a3=10500;
a4=14000;
a5=17500;
a6=21000;
a7=22200;
a8=25700;
a9=29200;
a10=32700;
a11=36200;
a12=39700;
a13=43200;
T1=3500;
T2=3500;
T3=1200;
s1=2204/36;
k1=s1/(T1*T1*T1);
p1=k1*(t1.^3);
k2=(3*s1)/(T1*T1);
p2=s1+((t2-a1)/T1)*3*s1+k2*((t2-a1).^2);
p3=7*s1+12*s1*(t3-a2)/T1-(s1-k1*((a3-t3).^3));
p4=18*s1+12*s1*(t4-a3)/T1-k1*((t4-a3).^3);
p5=32*s1+3*s1*(t5-a4)/T1-k2*((a5-t5).^2);
p6=36*s1-k1*((a6-t6).^3);
p7=36*s1-t7+t7;
k3=s1/((T2).^3);
p8=36*s1-k3*((t8-a7).^3);
k4=3*s1/((T2.^2));
p9=35*s1-(t9-a8)*3*s1/T2-k4*((t9-a8).^2);
p10=30*s1-(t10-a9)*12*s1/T2-k3*((a10-t10).^3);
p11=18*s1-(t11-a10)*12*s1/T2+k3*((t11-a10).^3);
p12=4*s1-(t12-a11)*3*s1/T2+k4*((a12-t12).^2);
p13=k3*((a13-t13).^3);
t=[t1,t2,t3,t4,t5,t6,t7,t8,t9,t10,t11,t12,t13];
p=[p1,p2,p3,p4,p5,p6,p7,p8,p9,p10,p11,p12,p13];
%subplot(2,2,1);
%plot(t,p);
%title('t,p');
B1=129.52568*pi/180;
B=B1-p*2*pi/43200;
%subplot(2,2,2);
%plot(t,B);
%title('t,B');
n=5413;
m=t-n;
x=98.5*sqrt(2)*cos(m*2*pi/43200)+99.16*cos(B+m*2*pi/43200);
y=98.5*sqrt(2)*sin(m*2*pi/43200)+99.16*sin(B+m*2*pi/43200);
%subplot(2,2,3);
%plot(x,y)
%axis('square')
%title('x,y');
%Q=[x y];
%Q
%x
%y
%t
dB=diff(B,'t');