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.2733    1.1170    1.4075    1.3836    1.1348    0.6830    0.9450];%輸入量
H_ZY_2=[1.2516    1.4499    1.8011    1.6056    1.5306    0.8843    1.0704];%輸入量

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=3;%可選擇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);

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

V1_2=-QAHTL_2(1)/(2*HL_2(1))*(TL_2(2)-T1_2).^2+QAHTL_2(2)/(2*HL_2(1))*(T1_2-TL_2(1)).^2+(PHTL_2(2)/HL_2(1)-HL_2(1)*QAHTL_2(2)/6)-(PHTL_2(1)/HL_2(1)-HL_2(1)*QAHTL_2(1)/6);
V2_2=-QAHTL_2(2)/(2*HL_2(2))*(TL_2(3)-T2_2).^2+QAHTL_2(3)/(2*HL_2(2))*(T2_2-TL_2(2)).^2+(PHTL_2(3)/HL_2(2)-HL_2(2)*QAHTL_2(3)/6)-(PHTL_2(2)/HL_2(2)-HL_2(2)*QAHTL_2(2)/6);
V3_2=-QAHTL_2(3)/(2*HL_2(3))*(TL_2(4)-T3_2).^2+QAHTL_2(4)/(2*HL_2(3))*(T3_2-TL_2(3)).^2+(PHTL_2(4)/HL_2(3)-HL_2(3)*QAHTL_2(4)/6)-(PHTL_2(3)/HL_2(3)-HL_2(3)*QAHTL_2(3)/6);
V4_2=-QAHTL_2(4)/(2*HL_2(4))*(TL_2(5)-T4_2).^2+QAHTL_2(5)/(2*HL_2(4))*(T4_2-TL_2(4)).^2+(PHTL_2(5)/HL_2(4)-HL_2(4)*QAHTL_2(5)/6)-(PHTL_2(4)/HL_2(4)-HL_2(4)*QAHTL(4)/6);
V5_2=-QAHTL_2(5)/(2*HL_2(5))*(TL_2(6)-T5_2).^2+QAHTL_2(6)/(2*HL_2(5))*(T5_2-TL_2(5)).^2+(PHTL_2(6)/HL_2(5)-HL_2(5)*QAHTL_2(6)/6)-(PHTL_2(5)/HL_2(5)-HL_2(5)*QAHTL(5)/6);
V6_2=-QAHTL_2(6)/(2*HL_2(6))*(TL_2(7)-T6_2).^2+QAHTL_2(7)/(2*HL_2(6))*(T6_2-TL_2(6)).^2+(PHTL_2(7)/HL_2(6)-HL_2(6)*QAHTL_2(7)/6)-(PHTL_2(6)/HL_2(6)-HL_2(6)*QAHTL(6)/6);
V7_2=-QAHTL_2(7)/(2*HL_2(7))*(TL_2(8)-T7_2).^2+QAHTL_2(8)/(2*HL_2(7))*(T7_2-TL_2(7)).^2+(PHTL_2(8)/HL_2(7)-HL_2(7)*QAHTL_2(8)/6)-(PHTL_2(7)/HL_2(7)-HL_2(7)*QAHTL(7)/6);


%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