-
Notifications
You must be signed in to change notification settings - Fork 0
/
GradientBasedPlanner3.m
40 lines (35 loc) · 1.62 KB
/
GradientBasedPlanner3.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
function route = GradientBasedPlanner3 (f, start_coords, end_coords, max_its)
% GradientBasedPlanner : This function plans a path through a 2D
% environment from a start to a destination based on the gradient of the
% function f which is passed in as a 2D array. The two arguments
% start_coords and end_coords denote the coordinates of the start and end
% positions respectively in the array while max_its indicates an upper
% bound on the number of iterations that the system can use before giving
% up.
% The output, route, is an array with 2 columns and n rows where the rows
% correspond to the coordinates of the robot as it moves along the route.
% The first column corresponds to the x coordinate and the second to the y coordinate
[gx, gy,gz] = gradient (-f);
%%% All of your code should be between the two lines of stars.
% *******************************************************************
route = start_coords;
pos = start_coords;
running = true;
i = 0;
%route = [start_coords(1), start_coords(2)];
while running
if (i == max_its) || (norm(end_coords - pos) < 2)
running = false;
break;
end
% while (i < max_its) && (f(pos(2),pos(1)) > )
disp(pos);
Delta = [ gx( round(pos(2)), round(pos(1)), round(pos(3)) ), ....
gy( round(pos(2)), round(pos(1)), round(pos(3)) ), ....
gz( round(pos(2)), round(pos(1)), round(pos(3)) )];
pos = pos + Delta/norm(Delta);
route = [route; pos];
i = i + 0.5;
end
% *******************************************************************
end