%??A?prorgam?to?find?the?torques?of?three?joints?for?movement?of
%??end-effector?from?source?location?to?goal?location
%??PID?controller?is?used?to?control?the?torqe?calculation?and?movement
%??cubic?polynomial?velocity?principle?is?applied?for?trajectory?Planning
%??By?:?Pranaya?Ranjan?Behera?Electronics?and?Instrumentation?Engg.
%??
%--------------------------------------------------------------------------

clear?all;
clc
g=9.8?;??????????%?in?mitre/sec^2
m2=?12;?????????????%?m2?and?m3?are?in?Kg;?mass?m1=0;
m3=?8;
l=?.68;???????????%?length?in?mitre??length1=0;?length2=length3=l;
kp=?1;
kv=?5;

ls=4*l*l;?
count=0;
while（count==0）
disp（‘Insert?the?source?location?of?end-effector（in?centi-miter）‘）
xs1=?input（‘Xs=‘）;
ys1=?input（‘Ys=‘）;
zs1=?input（‘Zs=‘）;??
xs=xs1/100;?ys=ys1/100;??zs=zs1/100;??%?converting?into?miter
%?check?singularity
ds2=（xs^2）+（ys^2）+（zs^2）;
if?（ls????disp（‘***?***?***?That?is?a?Singular?Point?***?***?***‘）;?
else
????count=1;
end???
end

while（count==1）
disp（‘Insert?the?destination（goal）?location?of?end-effector（in?centi-miter）‘）
xg1=?input（‘Xg=‘）;
yg1=?input（‘Yg=‘）;
zg1=?input（‘Zg=‘）;????
xg=xg1/100;?yg=yg1/100;??zg=zg1/100;?%?converting?into?miter
%?check?singularity
dg2=（xg^2）+（yg^2）+（zg^2）;
if?（ls????disp（‘***?***?***?That?is?a?Singular?Point?***?***?***‘）;
else
????count=0;
end
end
?????
while（count==0）?????
t=?input（‘Insert?time?of?movement（in?seconds）=‘）;
if?（t<.999）????%?time?of?travel?should?be?minimun?1sec.
????disp（‘***?***?***?Time?should?be?at?least?1?second?***?***?***‘）;
else
????count=1;
end
end


%??1?unit?=?50ms?i.e.??each?50ms?torques?will?be?caculated

qs1=atan2（ysxs）

qs3=acos（（（（（xs*?cos（qs1））+?（ys*?sin（qs1）））^2）?+?（zs^2）-（2*l*l）?）/?（2*l*l））

qs2=asin（（（zs*（1+cos（qs3））^2）-（（（zs*（1+cos（qs3））^2）^2）-?2*（1+cos（qs3））*（（zs^2）-（l^2）*（sin（qs3））^2））^0.5）/（2*l*（1+cos（qs3））））

%----------------------------------------------------------------

qg1=atan2（ygxg）

qg3=acos（（（（（xg*?cos（qg1））+?（yg*?sin（qg1）））^2）?+?（zg^2）-（2*l*l）?）/?（2*l*l））

qg2=asin（（（zg*（1+cos（qg3））^2）-（（（zg*（1+cos（qg3））^2）^2）-?2*（1+cos（qg3））*（（zg^2）-（l^2）*（sin（qg3））^2））^0.5）/（2*l*（1+cos（qg3））））


%-----------------------------------------------------------------
q1=q_cubic_fn（qs1?qg1?t）;
q2=q_cubic_fn（qs2?qg2?t）;
q3=q_cubic_fn（qs3?qg3?t）;

qd11=qd_cubic_fn（qs1?qg1?t）;
qd22=qd_cubic_fn（qs2?qg2?t）;
qd33=qd_cubic_fn（qs3?qg3?t）;

qdd1=qdd_cubic_fn（qs1?qg1?t）;
qdd2=qdd_cubic_fn（qs2?qg2?t）;
qdd3=qdd_cubic_fn（qs3?qg3?t）;



?for?i=1:20*t
?????qd1=qd11（i）;
?????qd2=qd22（i）;
?????qd3=qd33（i）;
?????qs1=?qd1^2;
?????qs2=?qd2^2;
?????qs3=?qd3^2;???????????%??square?of?qd?;
?????
?????c1=?cos（q1（i））;
?????s1=?sin（q1（i））;
?????c2=?cos（q2（i））;
?????s2=?sin（q2（i））;
?????c3=?cos（q3（i））;
?????s3=?sin（q3（i））;
?????s223=sin（q3（i）+?q2（i）+q2（i））;
?????c12=?cos（q1（i）+?q2（i））;
?????s12=?sin（q1（i）+?q2（i））;
?????c23=?cos（q3（i）+?q2（i））;

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????5485??2010-04-16?18:30??Force?Position?control?of?a?manipulator\main_program.m

?????文件???????1331??2014-03-19?14:54??Force?Position?control?of?a?manipulator\Matlab_Program_1.m

?????文件????????582??2010-03-31?19:54??Force?Position?control?of?a?manipulator\qdd_cubic_fn.m

?????文件????????589??2010-03-31?19:55??Force?Position?control?of?a?manipulator\qd_cubic_fn.m

?????文件???????1494??2010-04-16?21:16??Force?Position?control?of?a?manipulator\qd_trap_fn.m

?????文件????????578??2010-04-01?19:58??Force?Position?control?of?a?manipulator\q_cubic_fn.m

?????文件????1495267??2010-04-16?18:29??Force?Position?control?of?a?manipulator\report.docx

????..A..H.???????162??2014-03-19?14:43??Force?Position?control?of?a?manipulator\~$report.docx

?????目錄??????????0??2014-08-06?00:00??Force?Position?control?of?a?manipulator

-----------?---------??----------?-----??----

??????????????1505488????????????????????9