clc
clear
t0=0;
t1=2.5;
t=t0:0.1:t1;
q1=-0.3013*power(t,3)+8.3546;
t0=0;
t1=3;
t=t0:0.1:t1;
q2=0.4432*power(t,3)-2.2596*power(t,2)-5.6491*power(t,1)+3.647;
t0=0;
t1=3.6;
t=t0:0.1:t1;
q3=0.6917*power(t,5)-5.6873*power(t,4)+11.8226*power(t,3)+1.7293*power(t,2)-7.2402*power(t,1)-21.6704;
t0=0;
t1=2;
t=t0:0.1:t1;
q4=-1.8001*power(t,3)+9.8819*power(t,2)-15.6303*power(t,1)-10.741;
t0=0;
t1=2.5;
t=t0:0.1:t1;
q5=0.1225*power(t,3)-0.9186*power(t,2)+2.2964*power(t,1)-16.8745;
figure(1)
plot([0:0.1:2.5 2.5:0.1:5.5 5.5:0.1:9.1 9.1:0.1:11.1 11.1:0.1:13.6],[q1 q2 q3 q4 q5],'r','linewidth',1.5)
hold on
plot(0,q1(1),'sr')
hold on
plot(2.5,q2(1),'sr')
hold on
plot(5.5,q3(1),'sr')
hold on
plot(9.1,q4(1),'sr')
hold on
plot(11.1,q5(1),'sr')
hold on
xlabel('\itt/s','fontsize',5);
ylabel('关节角\itq/(^。)','fontsize',5);
legend('动臂关节','斗杆关节','铲斗关节',1);
t2=0;
t3=2.5;
t=t2:0.1:t3;
q6=-2.5300*power(t,3)+0.0009;
t2=0;
t3=3;
t=t2:0.1:t3;
q7=12.7259*power(t,3)-18.9752*power(t,2)-47.4380*power(t,1)-39.5308;
t2=0;
t3=3.6;
t=t2:0.1:t3;
q8=-11.7287*power(t,5)+103.8784*power(t,4)-263.6126*power(t,3)+95.5582*power(t,2)+182.3108*power(t,1)-9.0217;
t2=0;
t3=2;
t=t2:0.1:t3;
q9=29.3243*power(t,3)-146.0244*power(t,2)+157.4023*power(t,1)-57.7041;
t2=0;
t3=2.5;
t=t2:0.1:t3;
q10=-3.9895*power(t,3)+29.9215*power(t,2)-74.8037*power(t,1)-92.4028;
figure(1)
plot([0:0.1:2.5 2.5:0.1:5.5 5.5:0.1:9.1 9.1:0.1:11.1 11.1:0.1:13.6],[q6 q7 q8 q9 q10],'y','linewidth',1.5)
hold on
plot(0,q6(1),'dr')
hold on
plot(2.5,q7(1),'dr')
hold on
plot(5.5,q8(1),'dr')
hold on
plot(9.1,q9(1),'dr')
hold on
plot(11.1,q10(1),'dr')
hold on
xlabel('\itt/s','fontsize',5);
ylabel('关节角\itq/(^。)','fontsize',5);
legend('动臂关节','斗杆关节',1);