forked from rbenefo/PotentialFieldPlanner
-
Notifications
You must be signed in to change notification settings - Fork 0
/
runsim.m
142 lines (111 loc) · 3.46 KB
/
runsim.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
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
%% Setup
clear all
close all
addpath('utils')
addpath('maps')
%% Simulation Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Modify this part %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
start = [0,0,0,0,0,0];
goal = [0,0,pi/2,0,0,0];
mapToUse = 'map7.txt';
dynSim = true; % false for map only, set to true to enable dynamic obstacles
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Do not modify after this %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Load Map and Robot
load 'robot.mat' robot
map = loadmap(mapToUse);
map.start = start;
map.goal = goal;
%% Setup the Simulation
% If lynx global doesn't exist in the base workspace start Lynx
if(~evalin('base','exist(''lynx'',''var'')'))
startLynx = 1;
else
global lynx
if ~ishandle(lynx.hLinks)
startLynx = 1;
else
startLynx = 0;
end
end
% Uncomment the correct line to run in either simulation or hardware
if startLynx
lynxStart();
% lynxStart('Hardware','Legend','Port','COM3');
pause(1);
end
H = plotmap(map);
% Move the lynx to the start position
lynxServo(start);
pause(1);
%% Run the Simulation
% Initially not done and at start location
isDone = 0;
qCurr = start;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% [1] For dynamic simulation.
if dynSim
tic
oldtimeElasped = 0; % store the old time
timeElasped = 0;
T = 0; % total time
show = 0; % flag to show obstacle
count = 0; % show how many obstacle
obs = map.obstacles;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Modify this part %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
T_appear = 1; % time between obstacles appearing
T_stay = 2; % duration per obstacle
numAppear = 100; % number of obstacles appearing
dist = 20; % set the distance between LIDAR and obstacle, unit: mm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Do not modify after this %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dynVar.T = T;
dynVar.count = count;
dynVar.show = show;
dynFix.T_appear = T_appear;
dynFix.T_stay = T_stay;
dynFix.numAppear = numAppear;
dynFix.dist = dist;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% While not finished, take a step
while ~isDone
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% [2] For dynamic simulation.
if dynSim
dynVar.T = dynVar.T + timeElasped;
[dynVar,map,H] = dynamicObs(dynVar,dynFix,map,qCurr,robot,obs,H);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculate the potential field step
[qNext, isDone] = potentialFieldStep_11(qCurr, map, robot);
%%%%%%% Uncomment only if running pcode
% fname = 'simParams.json';
% fid = fopen(fname);
% simJSON = jsondecode(fscanf(fid, '%c', inf));
% fclose(fid);
%
% [qNext, isDone] = potentialFieldStep_sol(qCurr, map, robot,simJSON);
%
%
% %%%%%%%
% Take the step
lynxServo(qNext);
qCurr = qNext;
% Pause for simulation
pause(0.02); % You may change this to speed up the sim or slow it down
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% [3] For dynamic simulation
% time elapsed
if dynSim
timeElasped = toc - oldtimeElasped;
oldtimeElasped = toc;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end