Info

This question is closed. Reopen it to edit or answer.

Index in position 1 exceeds array bounds (must not exceed 641). Error in measurements (line 117) if m(xo,yo) ==0

1 view (last 30 days)
GHADAH AL-OBAIDI
GHADAH AL-OBAIDI on 21 Apr 2021
Closed: Stephen on 27 Apr 2021
This question was flagged by Paul Hoffrichter
I got the above error and I want to get the below output:
%%%% my code is:
close all
clear all
clc
xo=-16; yo=-16; %Map cooridnates
x=-13; y=-13; th=pi/2;
rr=1; %Robot radius
r=rr; %ray vector
n=3; %Number of sensors
dT=0.4;
dbstop if error
%%% My map
xw=-16; xe=16; ys=-16; yn=16; %map dimensions, west, east, south, north
mrx=0.05; mry=0.05; % map resolution {x,y}
[xm ym]=meshgrid(xw:mrx:xe,ys:mry:yn);
m=0*xm.*ym; % initialize the map m
for i=1:size(xm,2)
for j=1:size(ym,1)
r=sqrt(xm(1,i)^2+ym(j,1)^2);
ang=atan2(ym(j,1),xm(1,i))+pi;
if 9<=r && r<=10 && (pi/15<=ang && ang<=29*pi/15)
m(j,i)=1;
elseif 6<=r && r<=7 && ~(pi-pi/6<=ang && ang<=pi+pi/6)
m(j,i)=1;
elseif -14<=xm(1,i) && xm(1,i)<-9 && -2.8<=ym(j,1) && ym(j,1)<=-1.8
m(j,i)=1;
elseif -11<=xm(1,i) && xm(1,i)<-9 && 1.8<=ym(j,1) && ym(j,1)<=2.8
m(j,i)=1;
elseif -14<=xm(1,i) && xm(1,i)<-13 && -2.8<=ym(j,1) && ym(j,1)<=14
m(j,i)=1;
elseif -11<=xm(1,i) && xm(1,i)<-10 && 2.8<=ym(j,1) && ym(j,1)<=11
m(j,i)=1;
elseif -14<=xm(1,i) && xm(1,i)<14 && 14<=ym(j,1) && ym(j,1)<=15
m(j,i)=1;
elseif -11<=xm(1,i) && xm(1,i)<11 && 11<=ym(j,1) && ym(j,1)<=12
m(j,i)=1;
elseif 14<=xm(1,i) && xm(1,i)<15 && -15<=ym(j,1) && ym(j,1)<=15
m(j,i)=1;
elseif -5<=xm(1,i) && xm(1,i)<14 && -15<=ym(j,1) && ym(j,1)<=-14
m(j,i)=1;
elseif 10<=r && r<=16 && 0.95*pi/3.5<=ang && ang<=1.05*pi/3.5
m(j,i)=1;
elseif xm(1,i)<0.98*xw || 0.98*xe<xm(1,i) || ym(j,1)<0.98*ys || 0.98*yn<ym(j,1) % Boundaries
m(j,i)=1;
end
end
end
mesh(xm,ym,m); axis image; view(0, 90); %xlabel('x'); ylabel('y');
%%%%My control
t1=[0:dT:3]; % NOISELESS
u1=[0*ones(size(t1)); -1*ones(size(t1))];
t2=[0:dT:0.8]; % NOISELESS
u2=[1*ones(size(t2)); 0*ones(size(t2))];
t3=[0:dT:1.5]; % NOISELESS
u3=[0*ones(size(t3)); 1*ones(size(t3))];
t4=[0:dT:6]; % NOISELESS
u4=[1*ones(size(t4)); 0*ones(size(t4))];
t5=[0:dT:1.5]; % NOISELESS
u5=[0*ones(size(t5)); 1*ones(size(t5))];
t6=[0:dT:1.6]; % NOISELESS
u6=[1*ones(size(t6)); 0*ones(size(t6))];
t7=[0:dT:1.5]; % NOISELESS
u7=[0*ones(size(t7)); -1*ones(size(t7))];
t8=[0:dT:19]; % NOISELESS
u8=[1*ones(size(t8)); 0*ones(size(t8))];
t9=[0:dT:1.55]; % NOISELESS
u9=[0*ones(size(t9)); 1*ones(size(t9))];
t10=[0:dT:25.6]; % NOISELESS
u10=[1*ones(size(t10)); 0*ones(size(t10))];
t11=[0:dT:1.5]; % NOISELESS
u11=[0*ones(size(t11)); 0.98*ones(size(t11))];
t12=[0:dT:24]; % NOISELESS
u12=[1*ones(size(t12)); 0*ones(size(t12))];
t13=[0:dT:1.5]; % NOISELESS
u13=[0*ones(size(t13)); 0.98*ones(size(t13))];
t14=[0:dT:13]; % NOISELESS
u14=[1*ones(size(t14)); 0*ones(size(t14))];
t15=[0:dT:1.5]; % NOISELESS
u15=[0*ones(size(t15)); 0.98*ones(size(t15))];
t16=[0:dT:3.5]; % NOISELESS
u16=[1*ones(size(t16)); 0*ones(size(t16))];
t17=[0:dT:1.2]; % NOISELESS
u17=[0*ones(size(t17)); -0.94*ones(size(t17))];
t18=[0:dT:24]; % NOISELESS
u18=[1*ones(size(t18)); 1/8.2*ones(size(t18))];
t19=[0:dT:1.59]; % NOISELESS
u19=[0*ones(size(t19)); 1.01*ones(size(t19))];
t20=[0:dT:7.9]; % NOISELESS
u20=[1*ones(size(t20)); 0*ones(size(t20))];
u=[u1 u2 u3 u4 u5 u6 u7 u8 u9 u10 u11 u12 u13 u14 u15 u16 u17 u18 u19 u20]; % concatenate all controls into one
for t=1:2
v=u(1,t);
if u(2,t)~=0
w=u(2,t);
else
w=100*eps; % 100*eps because eps alone is too small, so causes division by zero problem
end
x=x-v/w*sin(th)+v/w*sin(th+w*dT);%state update
y=y+v/w*cos(th)-v/w*cos(th+w*dT);%state update
th=th+w*dT;%state update
subplot(1,2,1);
mesh(xm,ym,m); axis image; view(0, 90); hold on; xlabel('x'); ylabel('y');
plot(x,y,'ro','MarkerSize',10,'LineWidth',2); % DRAWS ROBOTs CIRCLE
plot([x; 1*r*cos(th)+x], [y; 1*r*sin(th)+y],'k-','LineWidth',2); % DRAWS BEARING
axis(16*[-1 1 -1 1]); drawnow; hold off;
%%%My measurement code
for k=0:n-1
th=th+2*pi*k/n;
xo=xo+r*cos(th); %corridnates(x,y) for the robot location
xo=((xo+16)/0.05)+1; xo=ceil(xo);
yo=yo+r*sin(th); %r is length(r)
yo=((yo+16)/0.05)+1; yo=ceil(yo);
if m(xo,yo) ==0 %if the location is not occupied
r=rr+0.05;
elseif m(xo,yo) == 1 %if the location is occupied
for kk=1:size(m,1)
subplot(1,2,2);
plot(m(kk,1) ,'b.'); axis(16*[-1 1 -1 1]); hold on;
end
end
end
end
  2 Comments

Answers (0)

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!