Merge branch 'master' of gitlab.liu.se:vehsys/tfyy51/yc4_2019

15 files changed, 127 insertions, 120 deletions

diff --git a/Kod/bilbana/files/17_bilbana1_2019-11-29T11.29.mat b/Kod/bilbana/files/17_bilbana1_2019-11-29T11.29.mat
new file mode 100644
index 0000000..393d76d
--- /dev/null
+++ b/Kod/bilbana/files/17_bilbana1_2019-11-29T11.29.mat
diff --git a/Kod/bilbana/files/Bana.mat b/Kod/bilbana/files/Bana.mat
new file mode 100644
index 0000000..38918a6
--- /dev/null
+++ b/Kod/bilbana/files/Bana.mat
diff --git a/Kod/bilbana/files/Bana1.mat b/Kod/bilbana/files/Bana1.mat
deleted file mode 100644
index e745c59..0000000
--- a/Kod/bilbana/files/Bana1.mat
+++ /dev/null
diff --git a/Kod/bilbana/files/Bana2.mat b/Kod/bilbana/files/Bana2.mat
deleted file mode 100644
index 56d1acb..0000000
--- a/Kod/bilbana/files/Bana2.mat
+++ /dev/null
diff --git a/Kod/bilbana/files/file_test.m b/Kod/bilbana/files/file_test.m
deleted file mode 100644
index 1370e7d..0000000
--- a/Kod/bilbana/files/file_test.m
+++ /dev/null
@@ -1,3 +0,0 @@
-Bana1
-car_position = 9.74;
-get_new_v(car_position, Bana1)
\ No newline at end of file
diff --git a/Kod/bilbana/files/grå_bilbana1_2019-11-29T10.41.mat b/Kod/bilbana/files/grå_bilbana1_2019-11-29T10.41.mat
new file mode 100644
index 0000000..7e14c4f
--- /dev/null
+++ b/Kod/bilbana/files/grå_bilbana1_2019-11-29T10.41.mat
diff --git a/Kod/bilbana/files/grå_bilbana1_2019-11-29T10.53.mat b/Kod/bilbana/files/grå_bilbana1_2019-11-29T10.53.mat
new file mode 100644
index 0000000..5bbd96b
--- /dev/null
+++ b/Kod/bilbana/files/grå_bilbana1_2019-11-29T10.53.mat
diff --git a/Kod/bilbana/files/grå_bilbana1_2019-11-29T11.20.mat b/Kod/bilbana/files/grå_bilbana1_2019-11-29T11.20.mat
new file mode 100644
index 0000000..d42c6c9
--- /dev/null
+++ b/Kod/bilbana/files/grå_bilbana1_2019-11-29T11.20.mat
diff --git a/Kod/bilbana/yc4/display/scenes/draw_lap_graph.m b/Kod/bilbana/yc4/display/scenes/draw_lap_graph.m
index 9fd669b..ca5ec83 100644
--- a/Kod/bilbana/yc4/display/scenes/draw_lap_graph.m
+++ b/Kod/bilbana/yc4/display/scenes/draw_lap_graph.m
@@ -83,15 +83,15 @@ if ~(in_clipboard)
 	car1.avg = '--.-';
 	car1.dev = '-.--';
 	if length(car1_laptimes) > 5
-		car1.avg = num2str(mean(car1_laptimes(6, length(car1_laptimes))), 3);
-		car1.dev = num2str(std(car1_laptimes(6, length(car1_laptimes))), 2);
+		car1.avg = num2str(mean(car1_laptimes(6:length(car1_laptimes))), 3);
+		car1.dev = num2str(std(car1_laptimes(6:length(car1_laptimes))), 2);
 	end
 	car2 = struct;
 	car2.avg = '--.-';
 	car2.dev = '-.--';
 	if length(car2_laptimes) > 5
-		car2.avg = num2str(mean(car2_laptimes(6, length(car2_laptimes))), 3);
-		car2.dev = num2str(std(car2_laptimes(6, length(car2_laptimes))), 2);
+		car2.avg = num2str(mean(car2_laptimes(6:length(car2_laptimes))), 3);
+		car2.dev = num2str(std(car2_laptimes(6:length(car2_laptimes))), 2);
 	end
 
 	line = 12;
diff --git a/Kod/bilbana/yc4/do_boot.m b/Kod/bilbana/yc4/do_boot.m
index 9c4930c..5b60eb8 100644
--- a/Kod/bilbana/yc4/do_boot.m
+++ b/Kod/bilbana/yc4/do_boot.m
@@ -5,29 +5,33 @@ if car.running == true
     %% BEFORE FIRST LAP
     if car.lap == 0
         t = toc(boot.time);
-        if t > 0.6
+        if t > 0.7
             car.constant = car.constant + 0.12;
-            disp('###')
-            disp(car.num)
-            disp(car.constant)
+            % disp('###')
+            % disp(car.num)
+            % disp(car.constant)
             boot.time = tic;
         end
     end
     %% WHEN NEW LAP
     if car.new_lap == 1
-        car.constant = car.constant * 1.2;
-        disp('###')
-        disp(car.num)
-        disp(car.constant)
+        car.constant = car.constant + 0.2;
+        % disp('###')
+        % disp(car.num)
+        % disp(car.constant)
     end
-    %% First segment
+    %% First segments
     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.04;
-            disp('###')
-            disp(car.num)
-            disp(car.constant)
+        if t > 1.2
+            if car.num == 1
+                car.constant = car.constant + 0.06;
+            else
+                car.constant = car.constant + 0.04;
+            end
+            % disp('###')
+            % disp(car.num)
+            % disp(car.constant)
             boot.time = tic;
         end
         
@@ -42,22 +46,20 @@ if car.running == true
  %            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);
-        car.constant = car.constant * 1.05;  % kompensation för kall bana
+        car.governs(length(car.governs) + 1) = car.constant;
+        % disp(car.constant);
+        status = car.forecasts_naive(car.lap, car.segment-1) / 15;
+        car.constant = car.constant + (status - 1) * 0.08;
+        
         boot.status = 0;
-        disp('END OF BOOTSTRAP')
-        disp(car.num)
-        disp(car.constant)
+        % disp('END OF BOOTSTRAP')
+        % disp(car.num)
+        % disp(car.constant)
+        car.governs(length(car.governs) + 1) = car.constant;
     end
 end
 end
\ No newline at end of file
diff --git a/Kod/bilbana/yc4/do_car.m b/Kod/bilbana/yc4/do_car.m
index 7728c28..92e4f8d 100644
--- a/Kod/bilbana/yc4/do_car.m
+++ b/Kod/bilbana/yc4/do_car.m
@@ -58,9 +58,9 @@ if car.running == true
         if toc(car.seg_tic) > 9.0
 			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');
+			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
@@ -73,10 +73,14 @@ if car.running == true
 	%% 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);
+		aprox_v = get_aprox_v(car.segment + detect_missed(car.position, car.segment, car.num, car.pos_at), car);
 		car.position = get_position(aprox_v, car.position, t);
+        if car.lap == 2 && car.segment < 4
+            % disp('####')
+            % disp(car.position)
+        end
         if detect_missed( car.position, car.segment, car.num, car.pos_at)
-			disp('Miss?');
+			% disp('Miss?');
 
 			%disp(toc(car.miss_time));
 			%if car.miss_time == 0
@@ -99,54 +103,51 @@ 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
-            
-            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;
+			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);
+			car.forecasts(car.lap, car.segment) = lap_time_now + track_remaining/prev_seg_v;
+
+			car.forecasts_naive(car.lap, car.segment) = toc(car.seg_tic) / car.percents(car.segment);
+
+			car.segment = min(9, car.segment + 1);
 			car.seg_tic = tic;
-            
-            if car.automatic && car.lap > 2 % S�kerhetsmarginal (B�r vara 1?)
-				disp(car)
+
+			if car.automatic && car.lap > 2 % S�kerhetsmarginal (B�r vara 1?)
 				[new_position, seg_plus] = ...
-						choose_position(car.position, car.segment, car.num, car.pos_at);
-                if seg_plus ~= 0 && car.segment == 2
+					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.segment = min(9, 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
+			else
 				car.position = car.pos_at(car.segment);
 				%car.miss_time = uint64(0);
-            end
-		end
+			end
+        end
+        disp(car)
 	end
 
 	%% NEW LAP
 	if car.new_lap == true
-        disp('NEW LAP')
-        
-        car.lap_constants = gov_set(car.constant);
-		% car.new_lap = false; %TODO remove
+		disp('NEW LAP')
 		beep;
-		if car.lap == 0
+        if car.lap == 0
 			% dont save time for first lap
 			car.segment = 1;
 			car.lap = car.lap + 1;
@@ -177,7 +178,8 @@ if car.running == true
 
 			car.segment = 1;
 			car.lap = car.lap + 1;
-		end
+        end
+        disp(car)
 	end
 end
 
@@ -189,14 +191,19 @@ 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));
+    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);
+	disp(strjoin({num2str(car.num), num2str(car.u), num2str(car.position)}))
+    if not(boot.status)
+        set_car_speed(car.num, max(car.u, 16));
+    else
+        set_car_speed(car.num, car.u);
+    end
 end
 end
diff --git a/Kod/bilbana/yc4/do_gov.m b/Kod/bilbana/yc4/do_gov.m
index 5f89c57..8ebea57 100644
--- a/Kod/bilbana/yc4/do_gov.m
+++ b/Kod/bilbana/yc4/do_gov.m
@@ -1,11 +1,30 @@
 function [ car ] = do_gov( car )
 %UNTITLED Summary of this function goes here
 %   Detailed explanation goes here
-if car.new_lap
-    % TODO calculate stuff
+if car.new_lap && car.lap > 1 % annars höjer den typ alltid första varvet
+	last_lap_time = car.lap_times(car.lap - 1);
+	time_diff = last_lap_time - car.ref_time;  % diff >0 => car is too slow, go faster
+	car.constant = car.constant + (time_diff / car.ref_time) * 0.1;
 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.08;
-    car.governs(length(car.governs) + 1) = car.constant;
+	if car.lap == 1
+		status = car.forecasts_naive(car.lap, car.segment-1)/car.ref_time;
+	else
+		status = car.forecasts(car.lap, car.segment-1)/car.ref_time;
+	end
+	car.constant = car.constant + (status - 1) * 0.08;
+	car.governs(length(car.governs) + 1) = car.constant;
+	%{
+	if car.lap > 1 && (car.segment == 5 || car.segment == 8)
+	car.lap_now = toc(car.lap_tic);
+	norm_const = 1/(sum(car.percents(1:9)));
+	norm_list = car.percents * norm_const;
+	sum_percent = sum(norm_list(1:car.segment));
+	exp_time = car.ref_time * sum_percent;
+
+	status = car.lap_now/exp_time;
+	car.constant = car.constant + (status - 1) * 0.08;
+	car.governs(length(car.governs) + 1) = car.constant;
+	end
+	%}
 end
 
diff --git a/Kod/bilbana/yc4/fit_percents.m b/Kod/bilbana/yc4/fit_percents.m
index ffdb9fc..7ccf0ba 100644
--- a/Kod/bilbana/yc4/fit_percents.m
+++ b/Kod/bilbana/yc4/fit_percents.m
@@ -8,4 +8,4 @@ for i = 1:length(percents)
     new_p = old_p - (old_p + cur_p) / 2;
     new_percents(i) = new_p;
 end
-
+new_percents = new_percents * (1/sum(new_percents))  % normera
diff --git a/Kod/bilbana/yc4/get_aprox_v.m b/Kod/bilbana/yc4/get_aprox_v.m
index 737dfac..5b6808b 100644
--- a/Kod/bilbana/yc4/get_aprox_v.m
+++ b/Kod/bilbana/yc4/get_aprox_v.m
@@ -1,4 +1,4 @@
-function [v] = get_aprox_v(cur_seg, lap, seg_times, track, seg_len)
+function [v] = get_aprox_v(cur_seg, car)
 %GET_APROX_V Tillf�llig? Beh�vs f�r att testa get_position. Ger
 %medelhastigheten f�r nuvarande segment p� f�rra varvet.
 %{
@@ -6,18 +6,21 @@ cur_seg: Nuvarande segment
 last_seg_times: 1x9 vektor med f�rra varvets segmenttider
 %}
 % seg_len = [];
-
+lap = car.lap;
 if cur_seg > 9
     cur_seg = cur_seg - 9;
 end
-
+%% Kompensera för väldigt långsamt segment 1 första varvet
+if cur_seg == 1 && lap == 2
+    v = car.seg_len(1)/(1.5*car.seg_times(1,9));
+    return
+end
+%% Sätt v
 while lap > 0
     lap = lap - 1;
-    if seg_times(lap, cur_seg) ~= 0
-        v = seg_len(cur_seg) / seg_times(lap, cur_seg);
+    if car.seg_times(lap, cur_seg) ~= 0
+        v = car.seg_len(cur_seg) / car.seg_times(lap, cur_seg);
         return
     end
 end
-disp('bara nollor?');
-
 end
diff --git a/Kod/bilbana/yc4/main.m b/Kod/bilbana/yc4/main.m
index f589c29..4ca058a 100644
--- a/Kod/bilbana/yc4/main.m
+++ b/Kod/bilbana/yc4/main.m
@@ -15,7 +15,7 @@ start_race(2)
 
 %% INIT
 global log_debug;
-log_debug = true;
+log_debug = false;
 global log_verbose;
 log_verbose = false;
 % INIT DISPLAY
@@ -37,8 +37,7 @@ display.shm_interp.data = [];
 display.last_send = tic;
 display.send_interval = 0.5;
 
-load('bilbana\files\Bana1.mat')
-load('bilbana\files\Bana2.mat')
+load('bilbana\files\Bana.mat')
 
 car1 = struct;
 car1.num = 1;
@@ -50,18 +49,18 @@ car1.segment = 1;
 car1.lap = 0;
 car1.lap_times = [];
 car1.seg_times = [];
-car1.seg_constant_list = []; % TODO
 car1.position = 0;
 car1.pos_at  = [0.0 2.53 3.05 4.73 7.68 8.98 10.93 14.69 17.57 19.60];
 car1.seg_len = [2.53 0.53 1.68 2.92 1.2 2.01 3.83 2.89 1.99];
-car1.percents = [];  % TODO
+car1.percents = [0.088, 0.022, 0.102, 0.15, 0.058, 0.11, 0.212, 0.146, 0.113];  % TODO
 car1.map = Bana1;
-car1.approximation = [];
-car1.miss_probability = 0.0;
-car1.lap_constants = [1,1,1,1,1,1,1,1,1]; % TODO
+%car1.approximation = [];
+car1.miss_probability = 0.05;
 car1.constant = 0.1;
 car1.stop = false;
 car1.governs = [];
+car1.forecasts = [];
+car1.forecasts_naive = [];
 
 car2 = struct;
 car2.num = 2;
@@ -79,13 +78,11 @@ 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.05;
-car2.seg_constant_list = []; % TODO
-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 = [];
-
+car2.forecasts = [];
+car2.forecasts_naive = [];
 
 boot1 = struct;
 boot1.status = false;
@@ -127,64 +124,48 @@ 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)
 			data = display.shm_interp.data(i);
 			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
@@ -307,12 +288,10 @@ while 1
     end
 end
 
-
 %% END OF RACE
 disp(highToc);
 disp(car1);
 disp(car2);
-
 terminate(1);
 terminate(2);