clear
D=[50,7,1000];%依次表示行,列,頁(yè)
N=3;n=8;%依次表示軸數(shù)和插值點(diǎn)數(shù)
%以P表示位置,V表示速度,A表示加速度,J表示加加速度
%T表示時(shí)間,h表示時(shí)間間隔
VC=[100,95,1000];%依次設(shè)置第一軸,第二軸,第三軸的速度約束
AC=[70,75,75];%依次設(shè)置第一軸,第二軸,第三軸的加速度約束
JC=[70,85,70];%依次設(shè)置第一軸,第二軸,第三軸的沖擊約束
V=zeros(N,n);%設(shè)置速度邊界
A=zeros(N,n);%設(shè)置加速度邊界
KT=1;KJ=0;
t1=0;%起始時(shí)間
Ps=0.04;Pm=0.16;Pc=0.8;
P(1,1)=120;P(1,3)=100;P(1,4)=60;P(1,5)=30;P(1,6)=10;P(1,8)=0;
P(2,1)=-10;P(2,3)=20;P(2,4)=0;P(2,5)=30;P(2,6)=60;P(2,8)=80;
P(3,1)=0;P(3,3)=20;P(3,4)=50;P(3,5)=70;P(3,6)=50;P(3,6)=30;
%以上三行設(shè)置初始位置點(diǎn)


H_ZY=[1.2733    1.1170    1.4075    1.3836    1.1348    0.6830    0.9450]
T_HT=SOV_T(H_ZY,n,t1)
QA_HT=SOL_QA(H_ZY,V,A,P,N,n);
P_HT=SOV_P(P,QA_HT,H_ZY,V,N,n);
[QJ_MAX,QJ_ZH]=SOV_QJ(QA_HT,H_ZY,N,n)

HTBL=2;%可選擇1,2,3表示第一,二,三軸
%for HTBL=1:3;
PHTL=P_HT(HTBL,:);%畫(huà)圖的位置
QAHTL=QA_HT(HTBL,:);%畫(huà)圖用的加速度
VL=V(HTBL,:);%zeros(1,n);%畫(huà)圖用的速度
HL=H_ZY;%畫(huà)圖用的時(shí)間間隔
TL=T_HT;%畫(huà)圖用的插值點(diǎn)時(shí)間

[QJ_MAX,QJ_ZH]=SOV_QJ(QA_HT,HL,N,n)%求出最大的沖擊

T1=TL(1):0.0001:TL(2);
T2=TL(2):0.0001:TL(3);
T3=TL(3):0.0001:TL(4);
T4=TL(4):0.0001:TL(5);
T5=TL(5):0.0001:TL(6);
T6=TL(6):0.0001:TL(7);
T7=TL(7):0.0001:TL(8);

Q1=QAHTL(1)/(6*HL(1))*(TL(2)-T1).^3+QAHTL(2)/(6*HL(1))*(T1-TL(1)).^3+(PHTL(2)/HL(1)-HL(1)*QAHTL(2)/6)*(T1-TL(1))+(PHTL(1)/HL(1)-HL(1)*QAHTL(1)/6)*(TL(2)-T1);
Q2=QAHTL(2)/(6*HL(2))*(TL(3)-T2).^3+QAHTL(3)/(6*HL(2))*(T2-TL(2)).^3+(PHTL(3)/HL(2)-HL(2)*QAHTL(3)/6)*(T2-TL(2))+(PHTL(2)/HL(2)-HL(2)*QAHTL(2)/6)*(TL(3)-T2);
Q3=QAHTL(3)/(6*HL(3))*(TL(4)-T3).^3+QAHTL(4)/(6*HL(3))*(T3-TL(3)).^3+(PHTL(4)/HL(3)-HL(3)*QAHTL(4)/6)*(T3-TL(3))+(PHTL(3)/HL(3)-HL(3)*QAHTL(3)/6)*(TL(4)-T3);
Q4=QAHTL(4)/(6*HL(4))*(TL(5)-T4).^3+QAHTL(5)/(6*HL(4))*(T4-TL(4)).^3+(PHTL(5)/HL(4)-HL(4)*QAHTL(5)/6)*(T4-TL(4))+(PHTL(4)/HL(4)-HL(4)*QAHTL(4)/6)*(TL(5)-T4);
Q5=QAHTL(5)/(6*HL(5))*(TL(6)-T5).^3+QAHTL(6)/(6*HL(5))*(T5-TL(5)).^3+(PHTL(6)/HL(5)-HL(5)*QAHTL(6)/6)*(T5-TL(5))+(PHTL(5)/HL(5)-HL(5)*QAHTL(5)/6)*(TL(6)-T5);
Q6=QAHTL(6)/(6*HL(6))*(TL(7)-T6).^3+QAHTL(7)/(6*HL(6))*(T6-TL(6)).^3+(PHTL(7)/HL(6)-HL(6)*QAHTL(7)/6)*(T6-TL(6))+(PHTL(6)/HL(6)-HL(6)*QAHTL(6)/6)*(TL(7)-T6);
Q7=QAHTL(7)/(6*HL(7))*(TL(8)-T7).^3+QAHTL(8)/(6*HL(7))*(T7-TL(7)).^3+(PHTL(8)/HL(7)-HL(7)*QAHTL(8)/6)*(T7-TL(7))+(PHTL(7)/HL(7)-HL(7)*QAHTL(7)/6)*(TL(8)-T7);

V1=-QAHTL(1)/(2*HL(1))*(TL(2)-T1).^2+QAHTL(2)/(2*HL(1))*(T1-TL(1)).^2+(PHTL(2)/HL(1)-HL(1)*QAHTL(2)/6)-(PHTL(1)/HL(1)-HL(1)*QAHTL(1)/6);
V2=-QAHTL(2)/(2*HL(2))*(TL(3)-T2).^2+QAHTL(3)/(2*HL(2))*(T2-TL(2)).^2+(PHTL(3)/HL(2)-HL(2)*QAHTL(3)/6)-(PHTL(2)/HL(2)-HL(2)*QAHTL(2)/6);
V3=-QAHTL(3)/(2*HL(3))*(TL(4)-T3).^2+QAHTL(4)/(2*HL(3))*(T3-TL(3)).^2+(PHTL(4)/HL(3)-HL(3)*QAHTL(4)/6)-(PHTL(3)/HL(3)-HL(3)*QAHTL(3)/6);
V4=-QAHTL(4)/(2*HL(4))*(TL(5)-T4).^2+QAHTL(5)/(2*HL(4))*(T4-TL(4)).^2+(PHTL(5)/HL(4)-HL(4)*QAHTL(5)/6)-(PHTL(4)/HL(4)-HL(4)*QAHTL(4)/6);
V5=-QAHTL(5)/(2*HL(5))*(TL(6)-T5).^2+QAHTL(6)/(2*HL(5))*(T5-TL(5)).^2+(PHTL(6)/HL(5)-HL(5)*QAHTL(6)/6)-(PHTL(5)/HL(5)-HL(5)*QAHTL(5)/6);
V6=-QAHTL(6)/(2*HL(6))*(TL(7)-T6).^2+QAHTL(7)/(2*HL(6))*(T6-TL(6)).^2+(PHTL(7)/HL(6)-HL(6)*QAHTL(7)/6)-(PHTL(6)/HL(6)-HL(6)*QAHTL(6)/6);
V7=-QAHTL(7)/(2*HL(7))*(TL(8)-T7).^2+QAHTL(8)/(2*HL(7))*(T7-TL(7)).^2+(PHTL(8)/HL(7)-HL(7)*QAHTL(8)/6)-(PHTL(7)/HL(7)-HL(7)*QAHTL(7)/6);

A1=QAHTL(1)/HL(1)*(TL(2)-T1)+QAHTL(2)/HL(1)*(T1-TL(1));
A2=QAHTL(2)/HL(2)*(TL(3)-T2)+QAHTL(3)/HL(2)*(T2-TL(2));
A3=QAHTL(3)/HL(3)*(TL(4)-T3)+QAHTL(4)/HL(3)*(T3-TL(3));
A4=QAHTL(4)/HL(4)*(TL(5)-T4)+QAHTL(5)/HL(4)*(T4-TL(4));
A5=QAHTL(5)/HL(5)*(TL(6)-T5)+QAHTL(6)/HL(5)*(T5-TL(5));
A6=QAHTL(6)/HL(6)*(TL(7)-T6)+QAHTL(7)/HL(6)*(T6-TL(6));
A7=QAHTL(7)/HL(7)*(TL(8)-T7)+QAHTL(8)/HL(7)*(T7-TL(7));

J1=(T1*(QAHTL(2)-QAHTL(1))/HL(1))./(T1+eps);
J2=T2*(QAHTL(3)-QAHTL(2))/HL(2)./T2;
J3=T3*(QAHTL(4)-QAHTL(3))/HL(3)./T3;
J4=T4*(QAHTL(5)-QAHTL(4))/HL(4)./T4;
J5=T5*(QAHTL(6)-QAHTL(5))/HL(5)./T5;
J6=T6*(QAHTL(7)-QAHTL(6))/HL(6)./T6;
J7=T7*(QAHTL(8)-QAHTL(7))/HL(7)./T7;

%subplot(3,1,HTBL)
plot(T1,Q1,'k-',T2,Q2,'k-',T3,Q3,'k-',T4,Q4,'k-',T5,Q5,'k-',T6,Q6,'k-',T7,Q7,'k-')%,legend('q','速度','加速度','沖擊')
hold on
%plot(T1,Q1,'k-',T2,Q2,'k-',T3,Q3,'k-',T4,Q4,'k-',T5,Q5,'k-')
plot(T1,V1,'k:',T2,V2,'k:',T3,V3,'k:',T4,V4,'k:',T5,V5,'k:',T6,V6,'k:',T7,V7,'k:')
plot(T1,A1,'k-.',T2,A2,'k-.',T3,A3,'k-.',T4,A4,'k-.',T5,A5,'k-.',T6,A6,'k-.',T7,A7,'k-.')
plot(T1,J1,'k--',T2,J2,'k--',T3,J3,'k--',T4,J4,'k--',T5,J5,'k--',T6,J6,'k--',T7,J7,'k--')
%xlabel('t/s');
%ylabel('位移,速度,加速度,沖擊');
hold off
%end;
%title('第三關(guān)節(jié)的位置、速度、加速度、沖擊曲線')
%grid