Kvällsarbete

Jag är hemskt ledsen över hur stor commiten blev.

12 files changed, 272 insertions, 212 deletions

diff --git a/Kod/bilbana/files/Bana2.mat b/Kod/bilbana/files/Bana2.mat
index 743de15..56d1acb 100644
--- a/Kod/bilbana/files/Bana2.mat
+++ b/Kod/bilbana/files/Bana2.mat
diff --git a/Kod/bilbana/yc4/choose_position.m b/Kod/bilbana/yc4/choose_position.m
index 5d2dee3..e6a3192 100644
--- a/Kod/bilbana/yc4/choose_position.m
+++ b/Kod/bilbana/yc4/choose_position.m
@@ -23,7 +23,10 @@ end
 disp(near);
 %% Beräkning av passerad givare
 seg_plus = max(0, near(1) - segment)
-new_position = track_len(segment + seg_plus);
+if segment + seg_plus < 10
+    new_position = track_len(segment + seg_plus);
+else
+    new_position = position;
 end
 
 
diff --git a/Kod/scripts/display_post_race_graphs.m b/Kod/bilbana/yc4/display/scenes/display_post_race_graphs.m
index 77336ce..a9da572 100644
--- a/Kod/scripts/display_post_race_graphs.m
+++ b/Kod/bilbana/yc4/display/scenes/display_post_race_graphs.m
@@ -1,10 +1,4 @@
-clear all;
-
-addpath display/ClientServerApp/Release
-cd display/ClientServerApp/Release
-!startServer
-cd ../../..
-
+function [] = display_post_race_graphs(seg_times1, seg_times2, lap_times1, lap_times2, ref_time)
 pause(1);
 
 matlabclient(1, get_smallpackage([ ...
@@ -15,16 +9,13 @@ matlabclient(1, get_smallpackage([ ...
 pause(0.2);
 
 %% CHECK DISPLAY BUTTONS
-display.last_check = tic;
+last_check = tic;
 done = false;
 
-laptime_1 = [12 12.1 12.2 12.3 12.4 12.5 12.6 12.7 12.8 12.9 13];
-laptime_2 = [11 11.2 11.4 11.6 11.8 12 12.5 13 13.5 14 14.2 14.6 14.8 15];
-
 while 1
 	pause(0.1);
-	if toc(display.last_check) > 0.4
-		display.last_check = tic;
+	if toc(last_check) > 0.4
+		last_check = tic;
 
 		% read internal mem from last send
 		[display.out, display.shm] = matlabclient(2);
@@ -37,19 +28,17 @@ while 1
 		end
 		update_ref_time = false;
 		for i = 1:length(display.shm_interp.data)
-			disp(num2str(length(display.shm_interp.data)))
 			data = display.shm_interp.data(i);
 			if data.data == 51
-				draw_lap_graph(laptime_1, laptime_2, 13, false);
+				draw_lap_graph(lap_times1, lap_times2, ref_time, false);
 			elseif data.data == 52
-				draw_segment_bars([1 2 3 4 5 6 7 8 9], [9 8 7 6 5 4 3 2 1]);
+				draw_segment_bars(seg_times1, seg_times2);
 			elseif data.data == 53
 				pause(0.2);
 				matlabclient(1, get_smallpackage(clear_display()));
-				pause(0.2);
 				done = true;
 			elseif data.data == 70
-				draw_lap_graph(laptime_1, laptime_2, 13, true);
+				draw_lap_graph(lap_times1, lap_times2, ref_time, true);
 			end
 		end
 		if done == true
@@ -62,5 +51,4 @@ while 1
 		display.last_check = tic;
 	end
 end
-
-matlabclient(3);
+end
diff --git a/Kod/bilbana/yc4/display/scenes/draw_lap_graph.m b/Kod/bilbana/yc4/display/scenes/draw_lap_graph.m
index ebee57e..eefd98e 100644
--- a/Kod/bilbana/yc4/display/scenes/draw_lap_graph.m
+++ b/Kod/bilbana/yc4/display/scenes/draw_lap_graph.m
@@ -1,5 +1,9 @@
 function [] = draw_lap_graph(car1_laptimes, car2_laptimes, ref_time, change_car)
 dt = 0.2;  % delay for display
+pause(2*dt);
+
+x_min = 40;
+dx_max = 232;
 
 % don't have to re-draw static elements, just the graphs
 persistent in_clipboard; 
@@ -15,7 +19,6 @@ if isempty(current_car)
 	current_car = 1;
 else
 	% switch current_car
-    disp('switching current car');
 	if change_car
 		if current_car == 1
 			current_car = 2;
@@ -42,14 +45,13 @@ for i = 1:length(car2_laptimes)
 end
 car2_laptimes = tmp;
 
-pause(dt);
 matlabclient(1, get_smallpackage(clear_display()));
 pause(dt);
 
 %% DRAW STATICS (OR COPY STATICS FROM CLIPBOARD)
 if ~(in_clipboard)
 	matlabclient(1, get_smallpackage([ ...
-			put_text(160, 8, 'C', 'Varvtider'), ...
+			% put_text(160, 8, 'C', 'Varvtider'), ...  % moved downwards
 			key(0  , 216, 107, 240, 51, 61, 'C', 'Varv'), ...
 			key(107, 216, 213, 240, 52, 62, 'C', 'Segment'), ...
 			key(213, 216, 320, 240, 53, 63, 'C', 'Avsluta'), ...
@@ -58,11 +60,11 @@ if ~(in_clipboard)
 	pause(dt);
 
 	matlabclient(1, get_smallpackage([ ...
-			draw_line(20, 24, 20, 144), ...   % y-axis
-			put_text(9, 25, 'C', 's'), ...  % label y-axis
+			draw_line(x_min, 24, x_min, 144), ...   % y-axis
+			put_text(x_min-11, 25, 'C', 's'), ...  % label y-axis
 			continue_line(304, 144), ... % x-axis
-			draw_line(16, 32, 20, 24), ...    % arrow on y, left part
-			continue_line(24, 32), ...   % arrow on y, right part
+			draw_line(x_min-4, 32, x_min, 24), ...    % arrow on y, left part
+			continue_line(x_min+4, 32), ...   % arrow on y, right part
 			draw_line(304, 140, 304, 148) ... % line on x
 	]));
 	pause(dt);
@@ -116,16 +118,14 @@ end
 
 laps = -1;
 % want to keep the same scale in x-axis for both cars so set laps to max of both
-laps = max(length(car1_laptimes), length(car2_laptimes));  % TODO check if max([]) == 0
-x_min = 20;
-dx_max = 260;
+laps = max(length(car1_laptimes), length(car2_laptimes));
 % x[lap] = x_min + (dx_max * (i / laps)))
 
 y_min_val = ref_time - 1.5;  % val for lowest y
 y_max_val = ref_time + 1.5;  % val for highest y
 y_min = 144;
 dy_max = -50;
-max_diff_val = 1;
+% max_diff_val = 1;  % unused
 y_mid = y_min + (dy_max);
 % y[lap] = clamp(y_min, y_mid + (dy_max * (lap[i] / ref_time)), y_min + 2*dy_max)
 
@@ -138,7 +138,8 @@ if current_car == 1 && ~isempty(car1_laptimes)
 elseif current_car == 2 && ~isempty(car2_laptimes)
 	times = car2_laptimes;
 	car = 2;
-% ignore current_car and choose the only carn_laptimes that exist
+% current_car doesn't have values ...
+% so ignore current_car and choose the only car_laptimes that exist
 elseif ~isempty(car1_laptimes)
 	times = car1_laptimes;
 	car = 1;
@@ -149,20 +150,36 @@ else
 	% not supposed to get here
 end
 
+matlabclient(1, get_smallpackage(put_text(160, 8, 'C', strjoin({'Varvtider bil', num2str(car)}))));
+pause(dt);
+
 for i = 1:(length(times))
 	x = round(x_min + (dx_max * (i/laps)));
 	y = clamp(round(y_mid + dy_max), ...
-			round(y_mid + (dy_max * ((times(i) - ref_time) / max_diff_val))), ...
+			round(y_mid + (dy_max * (times(i) - ref_time))), ...
 			round(y_mid - dy_max) ...
 	);
 	matlabclient(1, get_smallpackage([ ...
 			set_point_size(3, 3), ...
-			point(x, y), ...
+			point(x, y), ...  % value
 			set_point_size(1, 1), ...
-			draw_line(x, 144-2,  x, 144+2) ...
+			draw_line(x, 144-2,  x, 144+2) ...  % markers on x-axis
 	]));
 	pause(dt);
 end
+
+y_half_up = round(y_mid + (dy_max * 0.5));
+y_half_down = round(y_mid + (dy_max * -0.5));
+matlabclient(1, get_smallpackage([ ...
+    draw_line(x_min, y_mid, 304, y_mid), ...
+    draw_line(x_min, y_half_up, 304, y_half_up), ...
+    draw_line(x_min, y_half_down, 304, y_half_down), ...
+    put_text(x_min-4, y_mid - 4, 'R', num2str(ref_time)), ...
+    put_text(x_min-4, y_half_up - 4, 'R', num2str(ref_time + 0.5)), ...
+    put_text(x_min-4, y_half_down - 4, 'R', num2str(ref_time - 0.5)) ...
+]));
+pause(dt);
+
 max_lap_nums = 9;
 d_lap = floor(laps/max_lap_nums) + 1;
 % examples: 
diff --git a/Kod/bilbana/yc4/display/scenes/draw_segment_bars.m b/Kod/bilbana/yc4/display/scenes/draw_segment_bars.m
index 6df2dab..a40808b 100644
--- a/Kod/bilbana/yc4/display/scenes/draw_segment_bars.m
+++ b/Kod/bilbana/yc4/display/scenes/draw_segment_bars.m
@@ -1,8 +1,8 @@
 function [] = draw_segment_bars(values1, values2)
 dt = 0.2;  % delay for display
+pause(2*dt);
 queue = [];  % internal queue for packages to send to display
 
-pause(dt);
 matlabclient(1, get_smallpackage(clear_display()));
 pause(dt);
 
diff --git a/Kod/bilbana/yc4/display/switch.m b/Kod/bilbana/yc4/display/switch.m
deleted file mode 100644
index e45f512..0000000
--- a/Kod/bilbana/yc4/display/switch.m
+++ /dev/null
@@ -1,7 +0,0 @@
-function pkg = define_touch_switch(x1, y1, x2, y2, down_code, up_code, just, text)
-arg_location = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)];
-arg_text = double(strjoin({just; text}, ''));
-arg_null = 0;
-
-pkg = get_package('AK', [arg_location, down_code, up_code, arg_text, arg_null]);
-end
diff --git a/Kod/bilbana/yc4/display/update_bar_graph.m b/Kod/bilbana/yc4/display/update_bar_graph.m
index ffca923..23f1169 100644
--- a/Kod/bilbana/yc4/display/update_bar_graph.m
+++ b/Kod/bilbana/yc4/display/update_bar_graph.m
@@ -1,3 +1,3 @@
 function pkg = update_bar_graph(num, val)
-pkg = get_package('BA', [num, val])
+pkg = get_package('BA', [num, val]);
 end
diff --git a/Kod/bilbana/yc4/do_boot.m b/Kod/bilbana/yc4/do_boot.m
index 5019319..34ad43b 100644
--- a/Kod/bilbana/yc4/do_boot.m
+++ b/Kod/bilbana/yc4/do_boot.m
@@ -15,7 +15,7 @@ if car.running == true
     end
     %% WHEN NEW LAP
     if car.new_lap == 1
-        car.constant = car.constant + 0.4;
+        car.constant = car.constant * 1.2;
         disp('###')
         disp(car.num)
         disp(car.constant)
@@ -24,7 +24,7 @@ if car.running == true
     if car.lap == 1 && car.segment == 1 || car.lap == 1 && car.segment == 2
         t = toc(boot.time);
         if t > 0.8
-            car.constant = car.constant + 0.06;
+            car.constant = car.constant + 0.04;
             disp('###')
             disp(car.num)
             disp(car.constant)
@@ -32,27 +32,28 @@ if car.running == true
         end
         
     end
- %% ide höj carconstant så att den blir mer aggresivare ju längre tid som det går t.ex efter 3.5 s   
- %%    if car.lap == 1 && car.segment == 1 || car.lap == 1 && car.segment == 2
- %%        t = toc(boot.time);
- %%        if t > 1.0
- %%            car.constant = car.constant + 0.5;
- %%            disp('###')
- %%            disp(car.num)
- %%            disp(car.constant)
- %%            boot.time = tic;
- %%        end
- %%        
- %%    end    
+ % ide höj carconstant så att den blir mer aggresivare ju längre tid som går t.ex efter 3.5 s   
+ %    if car.lap == 1 && car.segment == 1 || car.lap == 1 && car.segment == 2
+ %        t = toc(boot.time);
+ %        if t > 1.0
+ %            car.constant = car.constant + 0.5;
+ %            disp('###')
+ %            disp(car.num)
+ %            disp(car.constant)
+ %            boot.time = tic;
+ %        end
+ %        
+ %    end    
     
     %% END BOOTSTRAP
     if car.segment > 3
         disp(car.constant);
-        seg_time = car.seg_times(1, 3)
-        laptime_forecast = seg_time / 0.102
-        forecast_ref_diff = laptime_forecast - car.ref_time
-        forecast_ref_diff_rel = forecast_ref_diff / car.ref_time
-        car.constant = car.constant + (forecast_ref_diff_rel * 0.15)
+        seg_time = car.seg_times(1, 3);
+        laptime_forecast = seg_time / 0.102;
+        forecast_ref_diff = laptime_forecast - car.ref_time;
+        forecast_ref_diff_rel = forecast_ref_diff / car.ref_time;
+        car.constant = car.constant + (forecast_ref_diff_rel * 0.15);
+        % car.constant = car.constant * 1.05;  % kompensation för kall bana
         boot.status = 0;
         disp('END OF BOOTSTRAP')
         disp(car.num)
diff --git a/Kod/bilbana/yc4/do_car.m b/Kod/bilbana/yc4/do_car.m
index 53c5235..82c4f40 100644
--- a/Kod/bilbana/yc4/do_car.m
+++ b/Kod/bilbana/yc4/do_car.m
@@ -71,7 +71,7 @@ if car.running == true
 	end
 
 	%% CALC POSITION
-	if car.lap > 1
+    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);
@@ -82,10 +82,10 @@ if car.running == true
 			%if car.miss_time == 0
 			%   car.miss_time = tic;
 			%end
-		end
-	end
+        end
+    end
 
-	if car.stopping == true
+	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)
@@ -99,23 +99,24 @@ if car.running == true
 	%% 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
+            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)
+            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)
+            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) = 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.lap > 2 % S�kerhetsmarginal (B�r vara 1?)
+            
+            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);
@@ -131,10 +132,10 @@ if car.running == true
                     disp(seg_plus)
                 end
 				%car.miss_time = uint64(0);
-			else
+            else
 				car.position = car.pos_at(car.segment);
 				%car.miss_time = uint64(0);
-			end
+            end
 		end
 	end
 
@@ -143,7 +144,7 @@ if car.running == true
         disp('NEW LAP')
         
         car.lap_constants = gov_set(car.constant);
-		car.new_lap = false; %TODO remove
+		% car.new_lap = false; %TODO remove
 		beep;
 		if car.lap == 0
 			% dont save time for first lap
@@ -163,9 +164,9 @@ if car.running == true
 			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,:))
+            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
+			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;
@@ -188,7 +189,10 @@ end
 
 %% CONTROLLER
 if car.running == true && car.automatic == false
-	% set_car_speed(car.num, mult * ((max - get_manual_speed(car.num)) / div));
+    mult = 100;
+    max = 55;
+    div = 55;
+	set_car_speed(car.num, mult * ((max - get_manual_speed(car.num)) / div));
 end
 
 %% EXECUTE
diff --git a/Kod/bilbana/yc4/do_gov.m b/Kod/bilbana/yc4/do_gov.m
index d38336a..5f89c57 100644
--- a/Kod/bilbana/yc4/do_gov.m
+++ b/Kod/bilbana/yc4/do_gov.m
@@ -1,9 +1,11 @@
 function [ car ] = do_gov( car )
 %UNTITLED Summary of this function goes here
 %   Detailed explanation goes here
-if car.new_check_point
+if car.new_lap
+    % TODO calculate stuff
+elseif car.new_check_point && (car.segment == 5 || car.segment == 8)
     status = car.forecasts(car.lap, car.segment-1)/car.ref_time;
-    car.constant = car.constant + (status - 1) * 0.01;
-    disp(car.constant);
+    car.constant = car.constant + (status - 1) * 0.08;
+    car.governs(length(car.governs) + 1) = car.constant;
 end
 
diff --git a/Kod/bilbana/yc4/format_seg_times.m b/Kod/bilbana/yc4/format_seg_times.m
new file mode 100644
index 0000000..5500250
--- /dev/null
+++ b/Kod/bilbana/yc4/format_seg_times.m
@@ -0,0 +1,20 @@
+function [ seg_times ] = format_seg_times( car )
+%FORMAT_POST_RACE_DATA Summary of this function goes here
+%   Detailed explanation goes here
+seg_times = [];
+disp(car.seg_times)
+if car.running
+    for x = 1:size(car.seg_times, 2)
+        s = 0;
+        amount = 0;
+        for y = 1:size(car.seg_times, 1)
+            if car.seg_times(y, x) ~= 0
+                s = s + car.seg_times(y, x);
+                amount = amount + 1;
+            end
+        end
+        seg_times(x) = s / amount;
+    end
+end
+end
+
diff --git a/Kod/bilbana/yc4/main.m b/Kod/bilbana/yc4/main.m
index 519fbf9..b8a1b32 100644
--- a/Kod/bilbana/yc4/main.m
+++ b/Kod/bilbana/yc4/main.m
@@ -1,5 +1,5 @@
 clear all;
-display_active = false;
+display_active = true;
 
 %% INIT TRACK
 disp('Startar bilbanan. Avsluta med q.')
@@ -20,10 +20,10 @@ global log_verbose;
 log_verbose = false;
 % INIT DISPLAY
 if display_active
-	addpath display/ClientServerApp/Release
-	cd display/ClientServerApp/Release
-	!startServer
-	cd ../../..
+    addpath display/ClientServerApp/Release
+    cd display/ClientServerApp/Release
+    !startServer
+    cd ../../..
 end
 
 display = struct;
@@ -61,6 +61,7 @@ car1.miss_probability = 0.0;
 car1.lap_constants = [1,1,1,1,1,1,1,1,1]; % TODO
 car1.constant = 0.1;
 car1.stop = false;
+car1.governs = [];
 
 car2 = struct;
 car2.num = 2;
@@ -76,7 +77,6 @@ car2.position = 0;
 car2.pos_at  = [0.0 2.53 3.05 4.92 7.62 9.02 10.72 14.68 17.76 19.95];
 car2.seg_len = [2.53 0.52 1.87 2.70 1.40 1.70 4.03 3.08 2.19];
 car2.percents = [0.088, 0.022, 0.102, 0.15, 0.058, 0.11, 0.212, 0.146, 0.113];
-
 car2.map = Bana2;
 car2.miss_probability = 0.1;
 car2.seg_constant_list = []; % TODO
@@ -84,14 +84,16 @@ car2.lap_constants = [1,1,1,1,1,1,1,1,1]; % TODO
 car2.seg_constant = 1;
 car2.constant = 0.1;
 car2.stop = false;
+car2.governs = [];
+
 
 boot1 = struct;
 boot1.status = false;
-boot1.time = 0;
+boot1.time = tic;
 
 boot2 = struct;
 boot2.status = false;
-boot2.time = 0;
+boot2.time = tic;
 
 halt = false;
 
@@ -100,21 +102,23 @@ ref_time = 13;
 t = 0;
 highToc = 0;
 
+pause(1);
+
 %% DRAW DISPLAY
 matlabclient(1, get_smallpackage([ ...
 	put_text(160, 30, 'C', 'Choose which car to drive'), ...
-	define_touch_switch(98 , 60 , 130, 90 , 11, 12, 'C', '1'), ...  % ACTIVATE TRACK 1
-	define_touch_switch(102, 98 , 126, 122, 13, 14, 'C', 'M'), ...  % MANUAL CONTROL TRACK 1
-	define_touch_switch(190, 60 , 222, 90 , 21, 22, 'C', '2'), ...  % ACTIVATE TRACK 2
-	define_touch_switch(194, 98 , 218, 122, 23, 61, 'C', 'M') ...  % MANUAL CONTROL TRACK 2
+	toggle(98 , 60 , 130, 90 , 11, 12, 'C', '1'), ...  % ACTIVATE TRACK 1
+	toggle(102, 98 , 126, 122, 13, 14, 'C', 'M'), ...  % MANUAL CONTROL TRACK 1
+	toggle(190, 60 , 222, 90 , 21, 22, 'C', '2'), ...  % ACTIVATE TRACK 2
+	toggle(194, 98 , 218, 122, 23, 61, 'C', 'M') ...  % MANUAL CONTROL TRACK 2
 	]));
 pause(0.5);
 matlabclient(1, get_smallpackage([ ...
-	put_text(160, 120, 'C', '13.0'), ...  % CURRENT REFERENCE TIME
-	define_touch_key(   98 , 110, 130, 140, 41, 42, 'C', '-'), ...  % DECREASE REFERENCE TIME
-	define_touch_key(   190, 110, 220, 140, 43, 44, 'C', '+'), ...  % INCREASE REFERENCE TIME
-	define_touch_key(   272, 192, 304, 224, 31, 32, 'C', 'S') ...  % START BUTTON
-	%TODO CLEAR BUTTON
+	put_text(160, 142, 'C', '13.0'), ...  % CURRENT REFERENCE TIME
+	key(98 , 130, 130, 160, 41, 42, 'C', '-'), ...  % DECREASE REFERENCE TIME
+	key(190, 130, 220, 160, 43, 44, 'C', '+'), ...  % INCREASE REFERENCE TIME
+	key(272, 192, 304, 224, 31, 32, 'C', 'S') ...  % START BUTTON
+	key(272, 192, 304, 224, 31, 32, 'C', 'S') ...  % START BUTTON
 	]));
 
 %% CHECK DISPLAY BUTTONS
@@ -123,48 +127,64 @@ done = false;
 while 1
 	pause(0.1);
 	if toc(display.last_check) > 0.4
+		verbose('DISPLAY', 'toc > 0.4');
 		display.last_check = tic;
 
 		% read internal mem from last send
 		[display.out, display.shm] = matlabclient(2);
 		[display.shm_interp.ack, display.shm_interp.start_code, display.shm_interp.data] = get_response(display.shm);
 
+		verbose('DISPLAY', 'Requesting internal mem for next cycle...');
 		% request internal mem
 		matlabclient(1, hex2dec(['12'; '01'; '53'; '66']));
+
 		if isempty(display.shm_interp.data)
+			verbose('DISPLAY', 'No response');
 			continue
 		end
+		debug('DISPLAY', ['Reading ', num2str(length(display.shm_interp.data)), ' package(-s)']);
+
 		update_ref_time = false;
 		for i = 1:length(display.shm_interp.data)
-			disp(num2str(length(display.shm_interp.data)))
 			data = display.shm_interp.data(i);
-			disp(data)
 			if data.data == 32
+				debug('DISPLAY', ...
+					'Start-button pressed, exiting when no more packages');
 				done = true;
 			elseif data.data == 11
+				debug('DISPLAY', 'Enabling car 1');
 				car1.running = true;
 			elseif data.data == 12
+				debug('DISPLAY', 'Disabling car 1');
 				car1.running = false;
 			elseif data.data == 13
+				debug('DISPLAY', 'Enabling car 1 manual');
 				car1.automatic = false;
 			elseif data.data == 14
+				debug('DISPLAY', 'Disabling car 1');
 				car1.automatic = true;
 			elseif data.data == 21
+				debug('DISPLAY', 'Enabling car 2');
 				car2.running = true;
 			elseif data.data == 22
+				debug('DISPLAY', 'Disabling car 2');
 				car2.running = false;
 			elseif data.data == 23
+				debug('DISPLAY', 'Enabling car 2 manual');
 				car2.automatic = false;
 			elseif data.data == 24
+				debug('DISPLAY', 'Disabling car 2 manual');
 				car2.automatic = true;
 			elseif data.data == 41
 				% ignore
 			elseif data.data == 42
+				debug('DISPLAY', ['Decreasing ref_time to ', num2str(ref_time)]);
 				ref_time = max(ref_time - 0.5, 12.0);
 				update_ref_time = true;
 			elseif data.data == 43
 				% ignore
 			elseif data.data == 44
+				debug('DISPLAY', ['Increasing ref_time to ', num2str(ref_time)]);
 				ref_time = min(ref_time + 0.5, 15.0);
 				update_ref_time = true;
 			end
@@ -174,139 +194,151 @@ while 1
 		end
 		if update_ref_time == true
 			pause(0.4);
-			matlabclient(1, get_smallpackage(put_text(160, 120, 'C', num2str(ref_time, '%.1f'))));
+			matlabclient(1, get_smallpackage(put_text(160, 142, 'C', num2str(ref_time, '%.1f'))));
 		end
 		display.last_check = tic;
 	end
 end
 
-debug('DISPLAY', ['CAR1.RUNNING=', car1.running, ', CAR1.AUTOMATIC=', car1.automatic])
-debug('DISPLAY', ['CAR2.RUNNING=', car2.running, ', CAR2.AUTOMATIC=', car2.automatic])
-debug('DISPLAY', ['CHOSEN REFERENCE TIME=', ref_time])
-
-matlabclient(1, get_smallpackage([define_bar_graph('O', 2, 266, 30, 290, 210, 0, 64, 1, 1)]));
+car1.ref_time = ref_time;
+car2.ref_time = ref_time;
 
-%% MAIN LOOP
-while 1
-	readTime = tic;
-	%% PRE-LOOP
-	if strcmp(get(hf,'currentcharacter'),'q')
-		close(hf)
-		break
-	elseif strcmp(get(hf, 'currentcharacter'), 's')
-		car1.stopping = true;
-		car2.stopping = true;
-	end
-end
-
-figure(hf)
-drawnow
+pause(0.5);
+matlabclient(1, get_smallpackage(clear_display()))
+pause(0.5);
 
-[car1, halt, display.data] = do_car(car1, t, display.data);
-if halt
-	break
-end
-[car2, halt, display.data] = do_car(car2, t, display.data);
-if halt
-	break
-end
+matlabclient(1, get_smallpackage([ ...
+    bar_graph('O', 1, 30, 30, 54, 210, 0, 64, 1, 1), ...
+    bar_graph('O', 2, 266, 30, 290, 210, 0, 64, 1, 1) ...
+    ]));
 
-%% BOOTSTRAP
-if boot1.status
-	[car1, boot1] = do_boot(car1, boot1);
-end
-if boot2.status
-	[car2, boot2] = do_boot(car2, boot2);
-end
-%% GOVERNOR
-if not(boot1.status) && car1.lap ~= 0
-	car1 = do_gov(car1);
-end
-if not(boot2.status) && car2.lap ~= 0
-	car2 = do_gov(car2);
-end
-%%
+boot1.status = car1.running && car1.automatic;
+boot2.status = car2.running && car2.automatic;
 
-if (~car2.running && car1.stopped) || (~car1.running && car2.stopped) || (car1.stopped && car2.stopped)
-	break;
+%% MAIN LOOP
+while 1
+    readTime = tic;
+    %% PRE-LOOP
+    if strcmp(get(hf,'currentcharacter'),'q')
+        close(hf)
+        break
+    elseif strcmp(get(hf, 'currentcharacter'), 's')
+        car1.stopping = true;
+        car2.stopping = true;
+    end
+    
+    figure(hf)
+    drawnow
+    
+    [car1, halt, display.data] = do_car(car1, t, display.data);
+    if halt
+        break
+    end
+    [car2, halt, display.data] = do_car(car2, t, display.data);
+    if halt
+        break
+    end
+    
+    %% BOOTSTRAP
+    if boot1.status
+        [car1, boot1] = do_boot(car1, boot1);
+    end
+    if boot2.status
+        [car2, boot2] = do_boot(car2, boot2);
+    end
+    %% GOVERNOR
+    if not(boot1.status) && car1.lap ~= 0
+        car1 = do_gov(car1);
+    end
+    if not(boot2.status) && car2.lap ~= 0
+        car2 = do_gov(car2);
+    end
+    %%
+    
+    if (~car2.running && car1.stopped) || (~car1.running && car2.stopped) || (car1.stopped && car2.stopped)
+        break;
+    end
+    
+    %% END OF LOOP
+    while 1                     %Whileloop med paus som k�rs till pausen �verskridit 0.07 sekunder
+        if display_active
+            % DISPLAY
+            display.send_delay = tic;
+            if toc(display.last_send) > display.send_interval && display_active
+                % queue control signal
+                if car1.running && car1.automatic
+                    % display.data = [display.data, put_text(20, 16 + (16 * 1), 'L', num2str(car1.u))];
+                    display.data = [display.data, update_bar_graph(1, car1.u)];
+                end
+                if car2.running && car2.automatic
+                    % display.data = [display.data, put_text(20, 16 + (16 * 2), 'L', num2str(car2.u))];
+                    display.data = [display.data, update_bar_graph(2, car2.u)];
+                end
+
+                % send all queued data
+                if ~isempty(display.data)
+                    [display.out] = matlabclient(1, get_smallpackage(display.data));
+                    display.data = [];
+                end
+                display.last_send = tic;
+
+                % read internal mem from last send
+                [display.out, display.shm] = matlabclient(2);
+                [display.shm_interp.ack, display.shm_interp.start_code, display.shm_interp.data] = get_response(display.shm);
+
+                % request internal mem
+                % matlabclient(1, hex2dec(['12'; '01'; '53'; '66']));
+            end
+        end
+        % disp(strjoin({'display took additional ', num2str(toc(display.send_delay))}));
+        % ACTUAL END OF LOOP
+        t = toc(readTime);
+        
+        if t > 0.07
+            if t > highToc
+                highToc = t;     %Om det nya v�rdet p� pausen �r h�gre �n den tidigare h�gsta s� sparas det som den h�gsta
+            end
+            if t > 0.1
+                % beep;
+            end
+            break;
+        end
+        pause(0.001);
+    end
 end
 
-%% END OF LOOP
-while 1                     %Whileloop med paus som k�rs till pausen �verskridit 0.07 sekunder
-	% DISPLAY
-	display.send_delay = tic;
-	if toc(display.last_send) > display.send_interval && display_active
-			% queue control signal
-			if car1.running && car1.automatic
-				% display.data = [display.data, put_text(20, 16 + (16 * 1), 'L', num2str(car1.u))];
-				display.data = [display.data, update_bar_graph(1, car1.u)];
-			end
-			if car2.running && car2.automatic
-				% display.data = [display.data, put_text(20, 16 + (16 * 2), 'L', num2str(car2.u))];
-				display.data = [display.data, update_bar_graph(2, car2.u)];
-			end
-
-			% send all queued data
-			if ~isempty(display.data)
-				[display.out] = matlabclient(1, get_smallpackage(display.data));
-				display.data = [];
-			end
-			display.last_send = tic;
-
-			% read internal mem from last send
-			[display.out, display.shm] = matlabclient(2);
-			[display.shm_interp.ack, display.shm_interp.start_code, display.shm_interp.data] = get_response(display.shm);
-
-			% request internal mem
-			% matlabclient(1, hex2dec(['12'; '01'; '53'; '66']));
-		end
-		% disp(strjoin({'display took additional ', num2str(toc(display.send_delay))}));
-		% ACTUAL END OF LOOP
-		t = toc(readTime);
-
-		if t > 0.07
-			if t > highToc
-				highToc = t;     %Om det nya v�rdet p� pausen �r h�gre �n den tidigare h�gsta s� sparas det som den h�gsta
-			end
-			if t > 0.1
-				% beep;
-			end
-			break;
-		end
-		pause(0.001);
-	end
-end
 
-%% END OF PROGRAM
+%% END OF RACE
 disp(highToc);
 disp(car1);
 disp(car2);
 
 terminate(1);
 terminate(2);
-if display_active
-	matlabclient(3);
-end
-%% DISPLAY GRAPHS
-
-if car1.running == true
-	graphs(car1.lap_times, car1.ref_time, car1.seg_times, 1);
-end
-
-
-if car2.running == true
-	graphs(car2.lap_times, car2.ref_time, car2.seg_times, 2);
-end
 
 %% SAVE VARIABLES FROM CAR STRUCT
 dateStr = datestr(now, 'yyyy-mm-dd');
 timeStr = datestr(now, 'HH.MM');
 
 if car1.lap > 2
-	filenameMat1 = strjoin({'bilbana1_', dateStr, 'T', timeStr, '.mat'}, '');
-	save(filenameMat1, '-struct', 'car1');
+    filenameMat1 = strjoin({'bilbana1_', dateStr, 'T', timeStr, '.mat'}, '');
+    save(filenameMat1, '-struct', 'car1');
 end
 if car2.lap > 2
-	filenameMat2 = strjoin({'bilbana2_', dateStr, 'T', timeStr, '.mat'}, '');
-	save(filenameMat2, '-struct', 'car2');
+    filenameMat2 = strjoin({'bilbana2_', dateStr, 'T', timeStr, '.mat'}, '');
+    save(filenameMat2, '-struct', 'car2');
+end
+
+%% DISPLAY POST RACE
+if display_active
+    % format segment times correctly
+    car1.seg_times_format = format_seg_times(car1);
+    car2.seg_times_format = format_seg_times(car2);
+    display_post_race_graphs(car1.seg_times_format, car2.seg_times_format, ...
+            car1.lap_times, car2.lap_times, ref_time);
+end
+
+%% CLEAN UP
+if display_active
+    matlabclient(3);
 end