Write a program using Matlab for Bug-2 (Robot): Robot goes towards the goal unti

Question

Write a program using Matlab for Bug-2 (Robot): Robot goes towards the goal until it hits an obstacle. The line from the start point to the goal point is called m-line. When an obstacle is encountered, the robot encircles the obstacle until it hits m-line. If the encountered point on m-line is nearer to the goal than the point that the robot starts following the obstacle then it stops following and starts moving towards the goal. The algorithm is terminated when the goal is reached.

There are several obstacles but here is one of them:

1.obstacle1.txt:

1
4
-8.052000 -6.720000
4.576000 7.933333
1.408000 8.353333
-11.000000 -5.040000
3 1 2
3 0 1

The numbers in the .txt files are obstacles that are given for this project. The robot needs to run successfully for every different obstacle. For more information and to get access to the obstacles' files please check (Project1: Bug Algorithms) http://www.robotmotionplanning.org/TeachingRobotics.html#Project1Bug

Explanation / Answer

function[x,y] = bug_planner( qstart,qgoal)

global sensor_range infinity;

stepp = (sensor_range * 0.075);
x=[ ]; y= [ ];
x = [x; qstart(1)];
y = [y; qstart(2)];
position=qstart;

hitdist = stepp*2;
small_turn= pi()/20.0;

angle_m = atan2((qgoal(2) - qstart(2)),(qgoal(1)-hitdist*0.5||..position(2)>qgoal(2)+hitdist*0.5||position(2)<qgoal(2)-hitdist*0.5)

if(mode==0)
angle = atan2(qgoal(2)-position(2),qgoal(1)-position(1));
position(1)= position(1)+cos(angle)*stepp;
position(2)= position(2)+sin(angle)*stepp;
x = [x position(1)];
y = [y position(2)];
dis = read_sensor(angle,[position(1) position(2)]);
if(dis<hitdist)
dist_to_goal = sqrt((qgoal(2) - position(2)) ^2+(qgoal(1)- position(1))^2)mode=1;
elseif(mode==1)
sensor_distance=[];
ang = linspace(0,2*pi( ),180);

for i = 1:180
sensor_distance = [sensor_distance;read_sensor(ang(i),position)];
end

[min_distance, ang_index] = min(sensor_distance);
ang_min = ang(ang_index);
min_distance = read_sensor(ang_min,position);

angle = ang_min-pi()/2;
angle = normalize_ang(angle);

if(min_distance<hitdist) angle = angle-small_turn;
elseif(min_distance>2*hitdist) angle = angle+small_turn;

position(1) = position(1)+ cos(angle)*stepp;
position(2) = position(2)+ sin(angle)* stepp;
x = [x position(1)];
y = [y position(2)];

angle = atan2(qgoal(2)-position(2),qgoal(1)-position(1));
angle = normalize_ang(angle);

if(abs(angle-angle_m)<pi()/180||abs(angle - angle_m-pi())<pi()/180||abs(angle- angle_m+pi()),pi()/180)

next obs = read sensor(angle,position);
if(next_obs>hitdist)
newdist_to_goal = sqrt((qgoal(2) - position(2))^2 +(qgoal(1) - position(1)
if(newdist_to_goal<dist_to_goal)
dist_to_goal = newdist_to_goal;
mode =0;
end
end
end

newdist_to_goal = sqrt((qgoal(2)-position(2))^2+(qgoal(1)- position(1))^2)
end
toc
end

Normalize_ang.m function
function[ang] = normalize_ang(angle);
ang = angle;
while (ang<0|| ang>2*pi())

if(ang<0) ang = ang+2*pi()
elseif(ang>2*pi()) ang = ang-2*pi();
end
end
end

Related Questions

Navigate

• Browse (All)
• Browse W
• Subjects

Integrity-first tutoring: explanations and feedback only — we do not complete graded work. Learn more.