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

1 files changed, 10 insertions, 7 deletions

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