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
|
%% INIT
% TODO init display
disp('Startar bilbanan. Avsluta med q.')
hf=figure('position',[0 0 eps eps],'menubar','none');
initialize_counters(1)
initialize_counters(2)
config_IOs
start_race(1)
start_race(2)
car1 = struct;
car1.segment = 1;
car1.lap = 0;
car1.lap_times = [];
car1.seg_times = [];
car2 = struct;
car2.segment = 1;
car2.lap = 0;
car2.lap_times = [];
car2.seg_times = [];
tocs = [];
%% MAIN LOOP
while 1
%% PRE-LOOP
if strcmp(get(hf,'currentcharacter'),'q')
close(hf)
break
end
figure(hf)
drawnow
%% READ
[car1.new_lap, car1.new_check_point, car1.time] = get_car_position(1);
[car2.new_lap, car2.new_check_point, car2.time] = get_car_position(2);
%% CHECK LAP AND CHECKPOINT (CAR 1)
if car1.new_check_point == true
% beep;
car1.seg_times(car1.lap, car1.segment) = -1; % TODO
car1.segment = car.segment + 1;
elseif car1.new_lap == true
beep;
if car1.lap == 0
% dont save time for first lap
car1.segment = 1;
continue;
end
car1.seg_times(car1.lap, car1.segment) = -1; % TODO
car1.lap_times(car1.lap) = -1; % TODO
car1.segment = 1;
car1.lap = car1.lap + 1;
end
%% CHECK LAP AND CHECKPOINT (CAR 2)
if car2.new_check_point == true
% beep;
car2.seg_times(car2.lap, car2.segment) = -1; % TODO
car2.segment = car.segment + 1;
elseif car2.new_lap == true
beep;
if car2.lap == 0
% dont save time for first lap
car2.segment = 1;
continue;
end
car2.seg_times(car2.lap, car2.segment) = -1; % TODO
car2.lap_times(car2.lap) = -1; % TODO
car2.segment = 1;
car2.lap = car2.lap + 1;
end
%% CALCULATE (CAR 1)
car1.car_constant = get_car_constant();
car1.v = get_new_v(car1.segment);
car1.track_u_constant = get_track_u_constant();
car1.u = get_new_u(car1.v, car1.car_constant, car1.track_u_constant);
%% CALCULATE (CAR 2)
car2.car_constant = get_car_constant();
car2.v = get_new_v(car2.segment);
car2.track_u_constant = get_track_u_constant();
car2.u = get_new_u(car2.v, car2.car_constant, car2.track_u_constant);
%% EXECUTE
set_car_speed(1, car1.u);
set_car_speed(2, car2.u);
%% DISPLAY
%% END OF LOOP
tocs(end + 1) = toc;
pause(0.1)
tic;
end
%% END OF PROGRAM
disp(car1);
disp(car2);
terminate(1);
terminate(2);
% TODO terminate display
|
