blob: 486c6d8c43b1ec26a6f90281f775dd34dfb163f8 (
plain) (
blame)
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
|
function [new_position, seg_plus] = choose_position(position,segment, track)
%CHOOSE_POSITION Välj vad position ska vara
% Kör endast vid ny indata. Kollar om indatan är rimlig eller om någon
% givare missats. Sedan väljs position efter vilken givare det var som
% passerades. seg_plus anger om och med hur mycket car.segment bör
% justeras för att kompensera efter missad givare.
track_len = [0 2.53 3.05 4.73 7.68 8.98 10.93 14.96 17.57;
0 0 0 0 0 0 0 0 0];
set_pos = [0 2.53 3.05 4.73 7.68 8.98 10.93 14.96 17.57;
0 0 0 0 0 0 0 0 0];
pos_c = position;
%% Vilken givare ligger närmast pos_c?
near = [];
for i = 1:length(track_len)
diff = abs(track_len(track,i)-pos_c);
if i == 1
near = [i,diff];
else
if diff < near(2)
near = [i,diff];
end
end
end
disp(near);
%% Beräkning av passerad givare
if near(1) == segment
new_position = set_pos( track, segment);
seg_plus = 0;
disp('In right segment');
else
if near(2) < 0.5
seg_plus = near(1) - segment
new_position = set_pos( track, segment + seg_plus)
beep
else
new_position = set_pos( track, segment); % ineff borde kombineras
seg_plus = 0;
disp('In right segment ich');
end
end
end
|
