clear
D=[50,7,1000];%依次表示行,列,頁
N=3;n=8;%依次表示軸數和插值點數
%以P表示位置,V表示速度,A表示加速度,J表示加加速度
%T表示時間,h表示時間間隔
VC=[100,95,1000];%依次設置第一軸,第二軸,第三軸的速度約束
AC=[70,75,75];%依次設置第一軸,第二軸,第三軸的加速度約束
JC=[70,85,70];%依次設置第一軸,第二軸,第三軸的沖擊約束
V=zeros(N,n);%設置速度邊界
A=zeros(N,n);%設置加速度邊界

t1=0;%起始時間

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,8)=30;

H_ZY=[1.2657    1.1209    1.4638    1.2874    1.0758    0.6938    0.9420];%輸入量
H_ZY_2=[1.2297    2.0498    2.4071    2.0203    1.6657    1.1749    1.2305];%輸入量

T_HT=SOV_T(H_ZY,n,t1);
T_HT_2=SOV_T(H_ZY_2,n,t1);

QA_HT=SOL_QA(H_ZY,V,A,P,N,n)
QA_HTSY=SOL_QASY(H_ZY,V,A,P,N,n)
QA_HT_2=SOL_QA(H_ZY_2,V,A,P,N,n);

P_HT=SOV_P(P,QA_HT,H_ZY,V,N,n);
P_HT_2=SOV_P(P,QA_HT_2,H_ZY_2,V,N,n);

HTBL=1;%可選擇1,2,3表示第一,二,三軸
%for HTBL=1:3;
PHTL=P_HT(HTBL,:);%畫圖的位置
PHTL_2=P_HT_2(HTBL,:);

QAHTL=QA_HT(HTBL,:);%畫圖用的加速度
QAHTL_2=QA_HT_2(HTBL,:);

VL=V(HTBL,:);%zeros(1,n);%畫圖用的速度

HL=H_ZY;%畫圖用的時間間隔
HL_2=H_ZY_2;

TL=T_HT;%畫圖用的插值點時間
TL_2=T_HT_2;

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

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

T1_2=TL_2(1):0.01:TL_2(2);
T2_2=TL_2(2):0.01:TL_2(3);
T3_2=TL_2(3):0.01:TL_2(4);
T4_2=TL_2(4):0.01:TL_2(5);
T5_2=TL_2(5):0.01:TL_2(6);
T6_2=TL_2(6):0.01:TL_2(7);
T7_2=TL_2(7):0.01:TL_2(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);

Q1_2=QAHTL_2(1)/(6*HL_2(1))*(TL_2(2)-T1_2).^3+QAHTL_2(2)/(6*HL_2(1))*(T1_2-TL_2(1)).^3+(PHTL_2(2)/HL_2(1)-HL_2(1)*QAHTL_2(2)/6)*(T1_2-TL_2(1))+(PHTL_2(1)/HL_2(1)-HL_2(1)*QAHTL_2(1)/6)*(TL_2(2)-T1_2);
Q2_2=QAHTL_2(2)/(6*HL_2(2))*(TL_2(3)-T2_2).^3+QAHTL_2(3)/(6*HL_2(2))*(T2_2-TL_2(2)).^3+(PHTL_2(3)/HL_2(2)-HL_2(2)*QAHTL_2(3)/6)*(T2_2-TL_2(2))+(PHTL_2(2)/HL_2(2)-HL_2(2)*QAHTL_2(2)/6)*(TL_2(3)-T2_2);
Q3_2=QAHTL_2(3)/(6*HL_2(3))*(TL_2(4)-T3_2).^3+QAHTL_2(4)/(6*HL_2(3))*(T3_2-TL_2(3)).^3+(PHTL_2(4)/HL_2(3)-HL_2(3)*QAHTL_2(4)/6)*(T3_2-TL_2(3))+(PHTL_2(3)/HL_2(3)-HL_2(3)*QAHTL_2(3)/6)*(TL_2(4)-T3_2);
Q4_2=QAHTL_2(4)/(6*HL_2(4))*(TL_2(5)-T4_2).^3+QAHTL_2(5)/(6*HL_2(4))*(T4_2-TL_2(4)).^3+(PHTL_2(5)/HL_2(4)-HL_2(4)*QAHTL_2(5)/6)*(T4_2-TL_2(4))+(PHTL_2(4)/HL_2(4)-HL_2(4)*QAHTL_2(4)/6)*(TL_2(5)-T4_2);
Q5_2=QAHTL_2(5)/(6*HL_2(5))*(TL_2(6)-T5_2).^3+QAHTL_2(6)/(6*HL_2(5))*(T5_2-TL_2(5)).^3+(PHTL_2(6)/HL_2(5)-HL_2(5)*QAHTL_2(6)/6)*(T5_2-TL_2(5))+(PHTL_2(5)/HL_2(5)-HL_2(5)*QAHTL_2(5)/6)*(TL_2(6)-T5_2);
Q6_2=QAHTL_2(6)/(6*HL_2(6))*(TL_2(7)-T6_2).^3+QAHTL_2(7)/(6*HL_2(6))*(T6_2-TL_2(6)).^3+(PHTL_2(7)/HL_2(6)-HL_2(6)*QAHTL_2(7)/6)*(T6_2-TL_2(6))+(PHTL_2(6)/HL_2(6)-HL_2(6)*QAHTL_2(6)/6)*(TL_2(7)-T6_2);
Q7_2=QAHTL_2(7)/(6*HL_2(7))*(TL_2(8)-T7_2).^3+QAHTL_2(8)/(6*HL_2(7))*(T7_2-TL_2(7)).^3+(PHTL_2(8)/HL_2(7)-HL_2(7)*QAHTL_2(8)/6)*(T7_2-TL_2(7))+(PHTL_2(7)/HL_2(7)-HL_2(7)*QAHTL_2(7)/6)*(TL_2(8)-T7_2);



%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('位移','速度','加速度','沖擊')
hold on
plot(T1_2,Q1_2,'k:',T2_2,Q2_2,'k:',T3_2,Q3_2,'k:',T4_2,Q4_2,'k:',T5_2,Q5_2,'k:',T6_2,Q6_2,'k:',T7_2,Q7_2,'k:'),%legend('位移','速度','加速度','沖擊')

%axis(0,11,-23,23)
%xlabel('t/s');
%axis tight
%ylabel('angle');
hold off
%end;
%title('第三關節的位置、速度、加速度、沖擊曲線')
%grid