环境:WIN7;MATLAB2010
加入了模拟光电编码器的计数显示
clc;close all;clear subplot(121);title('模拟机器人坐标'); hold on axis([0,130,0,200]); t= 0.01; T= 1; [X,Y] = ginput(1); C= pi/2; P= [X,Y,C]; L= 4; V= 4; Vl= V; Vr= V; Pl=[X-L/2,Y]; Pr=[X+L/2,Y]; plot((Pl(1)+Pr(1))/2,(Pl(2)+Pr(2))/2,'k*'); plot(Pl(1),Pl(2),'ro'); plot(Pr(1),Pr(2),'bo'); text(40,180,{strcat('P= ',num2str((Pl(1)+Pr(1))/2),', ',num2str((Pl(2)+Pr(2))/2))}); report(:,T)= [Vl;Vr;P(1);P(2)]; T= T+1; subplot(122); % plot(1:T-1,report(1,:)*10,'r.',1:T-1,report(2,:)*10,'b.',1:T-1,report(3,:),'g',1:T-1,report(4,:),'c');title('模拟光电编码器脉冲计数'); % legend('左轮计数','右轮计数','X','Y','Location','NorthWest'); pause(t); while(1) subplot(121); [Xt,Yt] = ginput(1); %在figure上用鼠标抓取数据,1表示抓取一次 PT = [Xt;Yt];%PT = [xt;yt;ct]目标点 plot(PT(1),PT(2),'k.','LineWidth',4); D= ((P(1)-PT(1)).^2+(P(2)-PT(2)).^2).^0.5; %((Pr(1)-Pl(1)).^2+(Pr(2)-Pl(2)).^2).^0.5 while (D>V)%与目标点距离大于一个速度继续移动 if (PT(1)>=P(1)&&PT(2)>=P(2)) if (PT(1)>=P(1)&&PT(2)>=P(2)) Ct = atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度 elseif(PT(1)<=P(1)&&PT(2)>=P(2)) Ct = pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度 elseif(PT(1)<=P(1)&&PT(2)<=P(2)) Ct = pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度 elseif(PT(1)>=P(1)&&PT(2)<=P(2)) Ct = atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度 end elseif(PT(1)<=P(1)&&PT(2)>=P(2)) if (PT(1)>=P(1)&&PT(2)>=P(2)) Ct = atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度 elseif(PT(1)<=P(1)&&PT(2)>=P(2)) Ct = pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度 elseif(PT(1)<=P(1)&&PT(2)<=P(2)) Ct = pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度 elseif(PT(1)>=P(1)&&PT(2)<=P(2)) Ct = atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度 end elseif(PT(1)<=P(1)&&PT(2)<=P(2)) if (PT(1)>=P(1)&&PT(2)>=P(2)) Ct = atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度 elseif(PT(1)<=P(1)&&PT(2)>=P(2)) Ct = pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度 elseif(PT(1)<=P(1)&&PT(2)<=P(2)) Ct = pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度 elseif(PT(1)>=P(1)&&PT(2)<=P(2)) Ct = 2*Pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度 end elseif(PT(1)>=P(1)&&PT(2)<=P(2)) if (PT(1)>=P(1)&&PT(2)>=P(2)) Ct = atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度 elseif(PT(1)<=P(1)&&PT(2)>=P(2)) Ct = pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度 elseif(PT(1)<=P(1)&&PT(2)<=P(2)) Ct = -pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度 elseif(PT(1)>=P(1)&&PT(2)<=P(2)) Ct = atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度 end end V= 0.2; w= 2*V/L; while(C>(Ct+0.5*w)||C<=(Ct-0.5*w))%角度调整 if (C>Ct) n = -1; else n = 1; end Vl= -1*n*V; Vr= n*V; dsl= L*sin(0.5*abs(w)); dsr= L*sin(0.5*abs(w)); if (PT(1)>=P(1)&&PT(2)>=P(2)) if (Pr(1)>=Pl(1)&&Pr(2)>=Pl(2)) c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)<=Pl(1)&&Pr(2)>=Pl(2)) c = pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)<=Pl(1)&&Pr(2)<=Pl(2)) c = -pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)>=Pl(1)&&Pr(2)<=Pl(2)) c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); end elseif (PT(1)<=P(1)&&PT(2)>=P(2)) if (Pr(1)>=Pl(1)&&Pr(2)>=Pl(2)) c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)<=Pl(1)&&Pr(2)>=Pl(2)) c = pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)<=Pl(1)&&Pr(2)<=Pl(2)) c = -pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)>=Pl(1)&&Pr(2)<=Pl(2)) c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); end elseif (PT(1)<=P(1)&&PT(2)<=P(2)) if (Pr(1)>=Pl(1)&&Pr(2)>=Pl(2)) c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)<=Pl(1)&&Pr(2)>=Pl(2)) c = pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)<=Pl(1)&&Pr(2)<=Pl(2)) c = pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)>=Pl(1)&&Pr(2)<=Pl(2)) c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); end elseif (PT(1)>=P(1)&&PT(2)<=P(2)) if (Pr(1)>=Pl(1)&&Pr(2)>=Pl(2)) c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)<=Pl(1)&&Pr(2)>=Pl(2)) c = -pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)<=Pl(1)&&Pr(2)<=Pl(2)) c = -pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)>=Pl(1)&&Pr(2)<=Pl(2)) c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); end end C= c+pi/2; Pl= Pl+[-1*n*dsl*cos(C+n*w/2),-1*n*dsl*sin(C+n*w/2)]; Pr= Pr+[n*dsr*cos(C+n*w/2),n*dsr*sin(C+n*w/2)]; % ((Pr(1)-Pl(1)).^2+(Pr(2)-Pl(2)).^2).^0.5 clf; subplot(121);title('模拟机器人坐标'); hold on axis([0,130,0,200]); text(20,180,{ strcat('目标位子:PT= ',num2str(PT(1)),', ',num2str(PT(2)),', ','Ct= ',num2str(Ct/pi),'*pi') strcat('机器人位子:P= ',num2str((Pl(1)+Pr(1))/2),', ',num2str((Pl(2)+Pr(2))/2),', ','C= ',num2str(C/pi),'*pi'), strcat('左右轮速度:Vl= ',num2str(Vl),', ','Vr= ',num2str(Vr),', ','w= ',num2str(w)), }); plot((Pl(1)+Pr(1))/2,(Pl(2)+Pr(2))/2,'k*'); plot(Pl(1),Pl(2),'ro'); plot(Pr(1),Pr(2),'bo'); plot(PT(1),PT(2),'k.','LineWidth',4); plot(report(3,:),report(4,:)); P= [(Pl(1)+Pr(1))/2,(Pl(2)+Pr(2))/2,C]; D= ((P(1)-PT(1)).^2+(P(2)-PT(2)).^2).^0.5; report(:,T)= [Vl;Vr;P(1);P(2)]; T= T+1; % subplot(122); % plot(1:T-1,report(1,:)*10,'r.',1:T-1,report(2,:)*10,'b.',1:T-1,report(3,:),'g',1:T-1,report(4,:),'c');title('模拟光电编码器脉冲计数'); % legend('左轮计数','右轮计数','X','Y','Location','NorthWest'); pause(t); end V= 4; Vl= V+(2*rand-1)*0.05*V; Vr= V+(2*rand-1)*0.05*V; w= (Vr-Vl)/L; Rl= abs(Vl/w); Rr= abs(Vr/w); dsl= 2*Rl*sin(0.5*abs(w)); dsr= 2*Rr*sin(0.5*abs(w)); if (PT(1)>=P(1)&&PT(2)>=P(2)) if (Pr(1)>=Pl(1)&&Pr(2)>=Pl(2)) c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)<=Pl(1)&&Pr(2)>=Pl(2)) c = pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)<=Pl(1)&&Pr(2)<=Pl(2)) c = -pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)>=Pl(1)&&Pr(2)<=Pl(2)) c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); end elseif (PT(1)<=P(1)&&PT(2)>=P(2)) if (Pr(1)>=Pl(1)&&Pr(2)>=Pl(2)) c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)<=Pl(1)&&Pr(2)>=Pl(2)) c = pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)<=Pl(1)&&Pr(2)<=Pl(2)) c = -pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)>=Pl(1)&&Pr(2)<=Pl(2)) c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); end elseif (PT(1)<=P(1)&&PT(2)<=P(2)) if (Pr(1)>=Pl(1)&&Pr(2)>=Pl(2)) c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)<=Pl(1)&&Pr(2)>=Pl(2)) c = pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)<=Pl(1)&&Pr(2)<=Pl(2)) c = pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)>=Pl(1)&&Pr(2)<=Pl(2)) c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); end elseif (PT(1)>=P(1)&&PT(2)<=P(2)) if (Pr(1)>=Pl(1)&&Pr(2)>=Pl(2)) c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)<=Pl(1)&&Pr(2)>=Pl(2)) c = -pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)<=Pl(1)&&Pr(2)<=Pl(2)) c = -pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); elseif(Pr(1)>=Pl(1)&&Pr(2)<=Pl(2)) c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1)); end end C= c+pi/2; Pl= Pl+[dsl*cos(C+w/2),dsl*sin(C+w/2)]; Pr= Pr+[dsr*cos(C+w/2),dsr*sin(C+w/2)]; clf; subplot(121);title('模拟机器人坐标'); hold on axis([0,130,0,200]); text(20,180,{ strcat('目标位子:PT= ',num2str(PT(1)),', ',num2str(PT(2)),', ','Ct= ',num2str(Ct/pi),'*pi') strcat('机器人位子:P= ',num2str((Pl(1)+Pr(1))/2),', ',num2str((Pl(2)+Pr(2))/2),', ','C= ',num2str(C/pi),'*pi'), strcat('左右轮速度:Vl= ',num2str(Vl),', ','Vr= ',num2str(Vr),', ','w= ',num2str(w)), }); plot((Pl(1)+Pr(1))/2,(Pl(2)+Pr(2))/2,'k*'); plot(Pl(1),Pl(2),'ro'); plot(Pr(1),Pr(2),'bo'); plot(PT(1),PT(2),'k.','LineWidth',4); plot(report(3,:),report(4,:)); P= [(Pl(1)+Pr(1))/2,(Pl(2)+Pr(2))/2,C]; D= ((P(1)-PT(1)).^2+(P(2)-PT(2)).^2).^0.5; report(:,T)= [Vl;Vr;P(1);P(2)]; T= T+1; % subplot(122); % plot(1:T-1,report(1,:)*10,'r.',1:T-1,report(2,:)*10,'b.',1:T-1,report(3,:),'g',1:T-1,report(4,:),'c');title('模拟光电编码器脉冲计数'); % legend('左轮计数','右轮计数','X','Y','Location','NorthWest'); pause(t); end subplot(122); plot(1:T-1,report(1,:)*10,'r.',1:T-1,report(2,:)*10,'b.',1:T-1,report(3,:),'g',1:T-1,report(4,:),'c');title('模拟光电编码器脉冲计数'); legend('左轮计数','右轮计数','X','Y','Location','NorthWest'); end
结果: