clear
clc
L1=link（[-pi/2?0?0?255?0]）;
%L?=link（[alpha?A?theta?D?sigma]）
%link.alpha	???返回扭轉角
%link.A????????返回桿件長度
%link.theta????返回關節角
%link.D????????返回橫距
%link.sigma????返回關節類型
L2=?link（[pi/2?0?0?255?0]）;
L3=link（[pi/2?0?0?0?0]）;
L4=link（[-pi/2?0?0?300?0]）;
L5=link（[pi/2?0?0?0?0]）;
L6=link（[0?0?0?120?0]）;
r=robot（{L1?L2?L3?L4?L5?L6}）;
r.name=‘MY?ROBOT‘;%模型的名稱
drivebot（r）;

q1=[0?0?0?0?0?0];%起始點關節空間矢量??????????
q2=[pi/12?-pi/6?pi/4?pi*5/12?pi*5/9?-pi*11/18];?%終止點關節空間矢量???
t=[0:0.05:10];%仿真時間??????????????
q=jtraj（q1q2t）;%關節空間規劃?????????
p=fkine（rq）;

x?=?squeeze（p（14:））;
y?=?squeeze（p（24:））;
z?=?squeeze（p（34:））;
s1?=?squeeze（q（:1:））;
s2?=?squeeze（q（:2:））;
s3?=?squeeze（q（:3:））;
s4?=?squeeze（q（:4:））;
s5?=?squeeze（q（:5:））;
s6?=?squeeze（q（:6:））;
%存入XYZ值
xlswrite（‘e:\測量件坐標.xlsx‘x‘sheet1‘‘a‘）;
xlswrite（‘e:\測量件坐標.xlsx‘y‘sheet1‘‘b‘）;
xlswrite（‘e:\測量件坐標.xlsx‘z‘sheet1‘‘c‘）;
%存入s1s2s3s4s5s6
xlswrite（‘e:\測量件旋轉角.xlsx‘s1‘sheet1‘‘a‘）;
xlswrite