rrrdrawing.m

6自由度机器人仿真程序

    hold on
    L22 = plot([Xdb(S2),Xb],[Ydb(S2) Yb],'Linewidth',2,'Marker','O');

    hold on
    L31 = plot([Xpc Xdc(S3)],[Ypc Ydc(S3)],'Linewidth',2,'Marker','O');
    text(Xpc-1,Ypc+1,'Base3','color',[0.9 0.7 0]);

    hold on 
    L32 = plot([Xdc(S3),Xc],[Ydc(S3) Yc],'Linewidth',2,'Marker','O');

    hold on

L41 = plot([Xa Xb],[Ya Yb],'color','r','Linewidth',3);
Horz= plot([Xa abs(Xb)+abs(H*sin(fai))],[Ya Ya],'color','b','LineStyle','--','color',[0 0.25 0.5]);
      text(Xa+0.7,Ya+0.5*sign(sin(fai)),'\phi','color',[0.5 0.25 0])

hold on
L42 = plot([Xb Xc],[Yb Yc],'color','y','Linewidth',3);

hold on
L43 = plot([Xc Xa],[Yc Ya],'color','g','Linewidth',3);
Center = plot(Xg,Yg,'o','color',[0.5 0 0.25]);


if (isempty(X)==0) & (isempty(Y)==0)
   for i=1:1:k-1
       hold on
       plot(X(i),Y(i),'m','marker','.')
       end
end 


axis([-15 15 -15 15])
grid on
hold off

if Yd(S1)>=Yp
theta11 = acos((Xd(S1)-Xp)/a1);
elseif Yd(S1)<Yp
    theta11 = 2*pi-acos((Xd(S1)-Xp)/a1);
end

if Ya>=Yd(S1)
theta12 = acos((Xa-Xd(S1))/b1);
elseif Ya<Yd(S1)
    theta12 = 2*pi-acos((Xa-Xd(S1))/b1);
end

if Ydb(S2)>=Ypb
theta21 = acos((Xdb(S2)-Xpb)/a1);
elseif Ydb(S2)<Ypb
    theta21 = 2*pi-acos((Xdb(S2)-Xpb)/a1);
end

if Yb>=Ydb(S2)
theta22 = acos((Xb-Xdb(S2))/b1);
elseif Yb<Ydb(S2)
    theta22 = 2*pi-acos((Xb-Xdb(S2))/b1);
end

if Ydc(S3)>=Ypc
theta31 = acos((Xdc(S3)-Xpc)/a1);
elseif Ydc(S3)<Ypc
    theta31 = 2*pi-acos((Xdc(S3)-Xpc)/a1);
end

if Yc>=Ydc(S3)
theta32 = acos((Xc-Xdc(S3))/b1);
elseif Yc<Ydc(S3)
    theta32 = 2*pi-acos((Xc-Xdc(S3))/b1);
end



if (Xd(S1)>Xa) & (Yd(S1)<Ya)
    diaota1 = theta11-theta12;
elseif (Xd(S1)<Xa) & (Yd(S1)<Ya)
    diaota1 = theta11+theta12;
elseif (Xd(S1)<Xa) & (Yd(S1)>Ya)
    diaota1 = theta12-theta11;
elseif (Xd(S1)>Xa) & (Yd(S1)>Ya)
    diaota1 = pi-(theta11-theta12);
end



if (Xdb>Xb) & (Ydb<Yb)
    diaota2 = theta21-theta22;
elseif (Xdb<Xb) & (Ydb<Yb)
    diaota2 = theta21+theta22;
elseif (Xdb<Xb) & (Ydb>Yb)
    diaota2 = theta22-theta21;
elseif (Xdb>Xb) & (Ydb>Yb)
    diaota2 = pi-(theta21-theta22);
end

if (Xdc>Xc) & (Ydc<Yc)
    diaota3 = theta31-theta32;
elseif (Xdc<Xc) & (Ydc<Yc)
    diaota3 = theta31+theta32;
elseif (Xdc<Xc) & (Ydc>Yc)
    diaota3 = theta32-theta31;
elseif (Xdc>Xc) & (Ydc>Yc)
    diaota3 = pi-(theta31-theta32);
end

angledisplay(AD)
else 
    
    warning('You facd a singularity. Please change the variables to solve it.')
    Warnd = text(-10,10,'You facd a singularity. Please change the variables to solve it.','color','b');
    set(Warnd,'visible','on')
    clc
end