模拟移动机器人控制(四)(再次修改)
2012-04-24 23:19
148 查看
环境: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
结果:
相关文章推荐
- 模拟移动机器人控制(四)(修改)
- 模拟移动机器人控制(一)
- 模拟移动机器人控制(四)
- 模拟移动机器人控制(二)
- 模拟移动机器人控制(三)
- CC2530基础实验之采集光照模拟量控制LED状态
- js控制修改radio选中项
- Java使用Map模拟事务控制-跨系统数据一致性处理
- Unity模拟龙之谷人物行走简单控制
- git-忽略版本控制内某些文件的修改
- iOS开发的一些小技术:让UIImage有缩放功能、控制log的输出、xcode修改文件注释、随机数的使用、在UIImageView 中旋转图像、在Quartz中如何设置旋转点、创建.plist文件并存储
- jquery 控制 select radio checkbox input 不可修改
- 控制一个 Java 程序运行生命周期结束前不能再次被执行
- 基于ZJOI 2012 Day2 T1的修改题: 破坏基地(方法:基于最短路做模拟)
- 模拟用iptables控制并发的http访问
- 金蝶K3修改库存更新控制方式
- 修改mtk平台power按键的gpio控制口
- 控制鼠标移到到特定控件上然后模拟鼠标单击
- [版本控制]如何在GitHub上下载自己喜欢的开源代码并提交修改