7 files changed, 26 insertions, 31 deletions

diff --git a/Kod/bilbana/yc4/choose_position.m b/Kod/bilbana/yc4/choose_position.m
index cd18f85..5d2dee3 100644
--- a/Kod/bilbana/yc4/choose_position.m
+++ b/Kod/bilbana/yc4/choose_position.m
@@ -1,16 +1,17 @@
-function [new_position, seg_plus] = choose_position(position,segment, track)
+function [new_position, seg_plus] = choose_position(position,segment, track, track_len)
 %CHOOSE_POSITION Välj vad position ska vara
 %   Kör endast vid ny indata. Kollar om indatan är rimlig eller om någon 
 %   givare missats. Sedan väljs position efter vilken givare det var som 
 %   passerades. seg_plus anger om och med hur mycket car.segment bör
 %   justeras för att kompensera efter missad givare.
-track_len = [0 2.53 3.05 4.73 7.68 8.98 10.93 14.96 17.57;
-             0 2.53 3.05 4.92 7.60 8.84 10.65 14.68 17.76];
 pos_c = position;
+
+track_len = track_len(1: length(track_len) - 1);
+
 %% Vilken givare ligger närmast pos_c?
 near = [];
 for i = 1:length(track_len)
-    diff = abs(track_len(track, i) - pos_c);
+    diff = abs(track_len(i) - pos_c);
     if i == 1
         near = [i, diff];
     else
@@ -22,7 +23,7 @@ end
 disp(near);
 %% Beräkning av passerad givare
 seg_plus = max(0, near(1) - segment)
-new_position = track_len(track, segment + seg_plus);
+new_position = track_len(segment + seg_plus);
 end
 
 
diff --git a/Kod/bilbana/yc4/detect_missed.m b/Kod/bilbana/yc4/detect_missed.m
index fb80d1c..1321920 100644
--- a/Kod/bilbana/yc4/detect_missed.m
+++ b/Kod/bilbana/yc4/detect_missed.m
@@ -1,9 +1,9 @@
-function [out] = detect_missed( position, segment, track)
+function [out] = detect_missed( position, segment, track, track_len)
 %DETECT_MISSED Retunerar true om position ligger utanför nuvarande segment
 %   
-track_len = [2.53 3.05 4.73 7.68 8.98 10.93 14.96 17.57 19.60;
-             2.53 3.05 4.92 7.60 8.84 10.65 14.68 17.76 19.95];
-a = track_len(track, segment);
+
+track_len = track_len(2: length(track_len));
+a = track_len(segment);
 out = a <= position;
 end
 
diff --git a/Kod/bilbana/yc4/do_car.m b/Kod/bilbana/yc4/do_car.m
index a280a86..f205f02 100644
--- a/Kod/bilbana/yc4/do_car.m
+++ b/Kod/bilbana/yc4/do_car.m
@@ -62,9 +62,9 @@ if car.running == true
 	%% 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.lap, car.seg_times, car.num);
+		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)
+		if detect_missed( car.position, car.segment, car.num, car.pos_at)
 			disp('Miss?');
 
 			%disp(toc(car.miss_time));
@@ -96,7 +96,7 @@ if car.running == true
 			if car.lap > 2 % S�kerhetsmarginal (B�r vara 1?)
 				disp(car)
 				[new_position, seg_plus] = ...
-						choose_position(car.position, car.segment, car.num);
+						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
@@ -110,7 +110,7 @@ if car.running == true
                 end
 				%car.miss_time = uint64(0);
 			else
-				car.position = car.seg_len(car.segment);
+				car.position = car.pos_at(car.segment);
 				%car.miss_time = uint64(0);
 			end
 		end
@@ -156,7 +156,7 @@ end
 %% CALCULATE
 if car.running == true && car.automatic == true
 	car.v = get_new_v(car.position, car.map);
-    seg_constant = get_seg_constant(car.position, car.lap_constants, car.num);
+    seg_constant = get_seg_constant(car.position, car.lap_constants, car.num, car.pos_at);
 	car.u = get_new_u(car.v, seg_constant);
 end
 
diff --git a/Kod/bilbana/yc4/get_aprox_v.m b/Kod/bilbana/yc4/get_aprox_v.m
index 1867565..737dfac 100644
--- a/Kod/bilbana/yc4/get_aprox_v.m
+++ b/Kod/bilbana/yc4/get_aprox_v.m
@@ -1,21 +1,12 @@
-function [v] = get_aprox_v(cur_seg, lap, seg_times, track)
+function [v] = get_aprox_v(cur_seg, lap, seg_times, track, seg_len)
 %GET_APROX_V Tillf�llig? Beh�vs f�r att testa get_position. Ger
 %medelhastigheten f�r nuvarande segment p� f�rra varvet.
 %{
 cur_seg: Nuvarande segment
 last_seg_times: 1x9 vektor med f�rra varvets segmenttider
 %}
-seg_len1 = [2.53 0.53 1.68 2.92 1.2 2.01 3.83 2.89 1.99];
-seg_len2 = [2.53 0.53 1.87 2.68 1.24 1.81 4.03 3.09 2.19];
-
 % seg_len = [];
 
-if track == 1
-    seg_len = seg_len1;
-else
-    seg_len = seg_len2;
-end
-
 if cur_seg > 9
     cur_seg = cur_seg - 9;
 end
diff --git a/Kod/bilbana/yc4/get_car_constant.m b/Kod/bilbana/yc4/get_car_constant.m
index 5513721..ae0bb0c 100644
--- a/Kod/bilbana/yc4/get_car_constant.m
+++ b/Kod/bilbana/yc4/get_car_constant.m
@@ -15,7 +15,7 @@ switch(in_pos)
     case 1
         car_constant = 0.95;
     case 2
-        car_constant = 0.8;
+        car_constant = 1.0;
 end
 end
 
diff --git a/Kod/bilbana/yc4/get_seg_constant.m b/Kod/bilbana/yc4/get_seg_constant.m
index 9f4dd1d..2de04ca 100644
--- a/Kod/bilbana/yc4/get_seg_constant.m
+++ b/Kod/bilbana/yc4/get_seg_constant.m
@@ -1,4 +1,4 @@
-function [out] = get_seg_constant(position, lap_constants, track)
+function [out] = get_seg_constant(position, lap_constants, track, track_len)
 %GET_SEG_CONSTANT Avgör vilken seg_constat som ska användas utifån
 %nuvarande position.
 %{   
@@ -6,10 +6,11 @@ position - Position i meter från start
 lap_constants - De seg_constants som ska användas detta varv
 track - den bana/bil som beräkningarna ska göras för
 %}
-track_len = [0 2.53 3.05 4.73 7.68 8.98 10.93 14.96 17.57;
-             0 2.53 3.05 4.92 7.60 8.84 10.65 14.68 17.76];
+
+track_len = track_len(1: length(track_len) - 1);
+
 for i = 1:length(track_len)
-    if position >= track_len(track, i)
+    if position >= track_len(i)
         seg_constant_num = i;
     end
 end
diff --git a/Kod/bilbana/yc4/main.m b/Kod/bilbana/yc4/main.m
index 2a551cd..9b31618 100644
--- a/Kod/bilbana/yc4/main.m
+++ b/Kod/bilbana/yc4/main.m
@@ -51,7 +51,8 @@ car1.lap_times = [];
 car1.seg_times = [];
 car1.seg_constant_list = []; % TODO
 car1.position = 0;
-car1.seg_len = [0.0 2.53 3.05 4.73 7.68 8.98 10.93 14.69 17.57];
+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.map = Bana1;
 car1.approximation = [];
 car1.miss_probability = 0.0;
@@ -68,7 +69,8 @@ car2.lap = 0;
 car2.lap_times = [];
 car2.seg_times = [];
 car2.position = 0;
-car2.seg_len = [0.0 2.53 3.05 4.92 7.60 8.84 10.65 14.68 17.76];
+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.map = Bana2;
 car2.miss_probability = 0.1;
 car2.seg_constant_list = []; % TODO