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
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
|
function [car, halt, 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.stopping - Logiskt värde som indikerar om bilen snart ska
stannas
car.stopped - Bilen har stannat och ska inte flyttas på
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.pos_at - Sträckan till målgivaren [m] från varje givare
(1 x 10 matris)
car.seg_len = Längden på varje segment [m] (1 x 9 matris)
car.percents - Upskattad andel av varvtiden som varje segment bör ta
(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.constant - Parameter som används för att sätta spänningen till banan
t - Längden (s) på nuvarande programcykel
display_data - Buffer med den data som ska skickas till displayen vid nästa
anrop
boot - En struct med info relaterad till bootstrap
boot.status - Logiskt värde som indikerar ifall bootstap körs eller ej
boot.time - Tiden sedan bootstrap höjde car.constant
halt - Huruvida koden ska stoppas eller inte
%}
halt = 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
halt = true;
return;
end
end
end
%% CALC POSITION
if car.automatic && 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.automatic && 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
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);
forecast = lap_time_now + track_remaining/prev_seg_v;
car.forecasts(car.lap, car.segment) = forecast;
car.segment = car.segment + 1;
car.seg_tic = tic;
if car.automatic && 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.automatic && 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
mult = 100;
max = 55;
div = 55;
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
|
