-
Notifications
You must be signed in to change notification settings - Fork 1
/
runsim.m
69 lines (55 loc) · 1.39 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
%% Setup
close all
addpath('utils')
addpath('maps')
%% Simulation Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Modify this part %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
start = [0,0,0,0,0,0];
goal = [pi/2,0,0,0,0,0];
mapToUse = 'map2.txt';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% 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
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;
% While not finished, take a step
while ~isDone
% Calculate the potential field step
[qNext, isDone] = potentialFieldStep_33(qCurr, map, robot);
% Take the step
lynxServo(qNext);
qCurr = qNext;
% Pause for simulation
pause(0.1); % You may change this to speed up the sim or slow it down
end