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_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);

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;

J1_2=(T1_2*(QAHTL_2(2)-QAHTL_2(1))/HL_2(1))./(T1_2+eps);
J2_2=T2_2*(QAHTL_2(3)-QAHTL_2(2))/HL_2(2)./T2_2;
J3_2=T3_2*(QAHTL_2(4)-QAHTL_2(3))/HL_2(3)./T3_2;
J4_2=T4_2*(QAHTL_2(5)-QAHTL_2(4))/HL_2(4)./T4_2;
J5_2=T5_2*(QAHTL_2(6)-QAHTL_2(5))/HL_2(5)./T5_2;
J6_2=T6_2*(QAHTL_2(7)-QAHTL_2(6))/HL_2(6)./T6_2;
J7_2=T7_2*(QAHTL_2(8)-QAHTL_2(7))/HL_2(7)./T7_2;

%subplot(3,1,HTBL)
plot(T1,J1,'k-',T2,J2,'k-',T3,J3,'k-',T4,J4,'k-',T5,J5,'k-',T6,J6,'k-',T7,J7,'k-')
%plot(T1,A1,'k-',T2,A2,'k-',T3,A3,'k-',T4,A4,'k-',T5,A5,'k-')
%plot(T1,V1,'k-',T2,V2,'k-',T3,V3,'k-',T4,V4,'k-',T5,V5,'k-')
%plot(T1,Q1,'k-',T2,Q2,'k-',T3,Q3,'k-',T4,Q4,'k-',T5,Q5,'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:'),%legend('位移','速度','加速度','沖擊')
%plot(T1_2,V1_2,'k:',T2_2,V2_2,'k:',T3_2,V3_2,'k:',T4_2,V4_2,'k:',T5_2,V5_2,'k:')
%plot(T1_2,A1_2,'k:',T2_2,A2_2,'k:',T3_2,A3_2,'k:',T4_2,A4_2,'k:',T5_2,A5_2,'k:')
plot(T1_2,J1_2,'k:',T2_2,J2_2,'k:',T3_2,J3_2,'k:',T4_2,J4_2,'k:',T5_2,J5_2,'k:',T6_2,J6_2,'k:',T7_2,J7_2,'k:')
%axis(0,11,-23,23)
%xlabel('t/s');
%axis tight
%ylabel('angle');
hold off
%end;
%title('第三關節的位置、速度、加速度、沖擊曲線')
%grid