模拟移动机器人控制(四)(再次修改)

环境: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
    

结果:


  • 1
    点赞
  • 3
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值