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
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
|
function [car, stop, display_data] = do_car(car, t, display_data, boot)
%DO_CAR Ger nya värden till struct car, avgör om koden ska stoppas samt hämtar displaydata.
%{
Input/Output:
car - En struct med data för en viss bil
car.num - Vilken bil det är (1 eller 2)
car.running - Om bilen körs eller inte
car.automatic - Om bilen körs automatiskt eller inte
car.segment - Bilens nuvarande segment
car.lap - Bilens nuvarande varv
car.lap_times - Bilens sparade varvtider (1 x v matris)
car.seg_times - Bilens sparade segmentstier (v x 9 matris)
car.seg_constant_list = []; % TODO Sparar alla seg_constants som
använts (v x 9 matris)
car.position - Bilens nuvarande placering på banan i meter från
start/mål
car.seg_len - Banans längd från start till givarna (1 x 9 matris)
car.map - Tabell med hastighetskoefficienter för alla positioner (.mat
fil)
car.miss_probability - Sannorlikheten för artificiellt introducerade
missade givare
car.lap_constants = [1,1,1,1,1,1,1,1,1]; % TODO seg_constanst för
nuvarande varv. Skapas av gov_set() vid nytt varv
t - Längden (s) på nuvarande programcykel
display_data - Buffer med den data som ska skickas till displayen vid nästa
anrop
stop - Huruvida koden ska stoppas eller inte
%}
stop = false;
if car.running == true
[car.new_lap, car.new_check_point, car.time] = get_car_position(car.num);
if car.new_check_point == true && rand < car.miss_probability && car.lap >= 4
disp('Hoppar �ver givare');
car.new_check_point = false;
beep;
end
end
if car.stopped == true
return
end
%% READ INPUT FROM TRACK
if car.running == true
if car.lap ~= 0
if toc(car.seg_tic) > 9.0 && not(boot.status)
set_car_speed(1, 0);
set_car_speed(2, 0);
%disp(strjoin({'Avåkning bil', num2str(car.num)}));
disp('J = Ja, N = Nej')
car.response = input('Vill du fortsätta? [N] ', 's');
if car.response == 'J'
car.seg_tic = tic;
else
stop = true;
return;
end
end
end
%% CALC POSITION
if car.lap > 1
% car.last_seg_times = car.seg_times(car.lap - 1, 1:9);
aprox_v = get_aprox_v(car.segment + detect_missed(car.position, car.segment, car.num, car.pos_at), car.lap, car.seg_times, car.num, car.seg_len);
car.position = get_position(aprox_v, car.position, t);
if detect_missed( car.position, car.segment, car.num, car.pos_at)
disp('Miss?');
%disp(toc(car.miss_time));
%if car.miss_time == 0
% car.miss_time = tic;
%end
end
end
if car.stopping == true
% CHECK IF CAR IS AT THE END OF TRACK
if car.position > (car.map(80, 1) / 100) - 0.8 % 80cm
disp(car.position)
disp((car.map(80, 1) / 100) - 300)
set_car_speed(car.num, 0);
car.stopped = true;
return
end
end
%% CHECK POINT
if car.new_check_point == true
if car.new_lap == false % choose_position krachar vid nytt varv (seg 10)
if car.lap ~= 0
car.seg_times(car.lap, car.segment) = toc(car.seg_tic);
end
seg_time = car.seg_times(car.lap, car.segment)
lap_time_now = toc(car.lap_tic)
% s = vt
% v = s/t
% t = s/v
prev_seg_v = car.seg_len(car.segment) / toc(car.seg_tic)
track_remaining = car.pos_at(length(car.pos_at)) - car.pos_at(car.segment + 1)
car.forecasts(car.lap, car.segment) = lap_time_now + track_remaining/prev_seg_v
car.segment = car.segment + 1;
car.seg_tic = tic;
if car.lap > 2 % S�kerhetsmarginal (B�r vara 1?)
disp(car)
[new_position, seg_plus] = ...
choose_position(car.position, car.segment, car.num, car.pos_at);
if seg_plus ~= 0 && car.segment == 2
disp('Hoppar �ver missad givare 1/2');
else
car.position = new_position;
car.segment = car.segment + seg_plus;
end
if seg_plus ~= 0 && car.segment ~= 2
car.seg_times(car.lap, car.segment - seg_plus - 1) = 0;
disp(car.seg_times(car.lap, :))
disp(seg_plus)
end
%car.miss_time = uint64(0);
else
car.position = car.pos_at(car.segment);
%car.miss_time = uint64(0);
end
end
end
%% NEW LAP
if car.new_lap == true
disp('NEW LAP')
car.lap_constants = gov_set(car.constant);
car.new_lap = false; %TODO remove
beep;
if car.lap == 0
% dont save time for first lap
car.segment = 1;
car.lap = car.lap + 1;
car.seg_tic = tic;
car.lap_tic = tic;
else
% beep;
% Spara inte seg_time om missad givare
if car.segment == 9
car.seg_times(car.lap, car.segment) = toc(car.seg_tic);
end
car.seg_tic = tic;
car.lap_times(car.lap) = toc(car.lap_tic);
car.lap_tic = tic;
car.position = 0;
% save segment percentage from last lap
car.percents = fit_percents(car.percents, car.lap_times(car.lap), car.seg_times(car.lap,:))
if car.lap == 1 && size(car.seg_times, 2) < 9
disp('FEL: För få segment!!')
car.stopped = true;
other_car.stopped = true;
return
end
display_data = [display_data, put_text(100, 16 + (16 * car.num), 'L', strjoin({num2str(car.lap), get_time_as_string(round(car.lap_times(car.lap) * 1000))}, ' '))];
car.segment = 1;
car.lap = car.lap + 1;
end
end
end
%% CALCULATE
if car.running == true && car.automatic == true
car.v = get_new_v(car.position, car.map);
car.u = get_new_u(car.v, car.constant);
end
%% CONTROLLER
if car.running == true && car.automatic == false
% set_car_speed(car.num, mult * ((max - get_manual_speed(car.num)) / div));
end
%% EXECUTE
if car.running == true && car.automatic == true
set_car_speed(car.num, car.u);
end
end
|
