4 files changed, 104 insertions, 12 deletions

diff --git a/Kod/bilbana/yc4/choose_position.m b/Kod/bilbana/yc4/choose_position.m
new file mode 100644
index 0000000..39be6e4
--- /dev/null
+++ b/Kod/bilbana/yc4/choose_position.m
@@ -0,0 +1,41 @@
+function [new_position, seg_plus] = choose_position(position,segment, track)
+%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 0 0 0 0 0 0 0 0];
+set_pos = [0 2.53 3.05 4.73 7.68 8.98 10.93 14.96 17.57;
+            0 0 0 0 0 0 0 0 0];
+pos_c = position;
+%% Vilken givare ligger närmast pos_c?
+near = [];
+for i = 1:length(track_len)
+    diff = abs(track_len(track,i)-pos_c);
+    if i == 1
+        near = [i,diff];
+    else
+        if diff < near(2)
+            near = [i,diff];
+        end
+    end
+end
+disp(near);
+%% Beräkning av passerad givare
+if near(1) == segment
+    new_position = set_pos( track, segment);
+    seg_plus = 0;
+    disp('In right segment');
+else
+    if near(2) < 0.7 % Behöver bli smartare. Typ jämföra andra normal miss med denna miss
+        seg_plus = max(0, near(1) - segment)
+        new_position = set_pos( track, segment + seg_plus);
+    else
+        new_position = set_pos( track, segment); % ineff borde kombineras
+        seg_plus = 0;
+        disp('In right segment ich');
+    end
+end
+end
+
diff --git a/Kod/bilbana/yc4/detect_missed.m b/Kod/bilbana/yc4/detect_missed.m
new file mode 100644
index 0000000..caad2ac
--- /dev/null
+++ b/Kod/bilbana/yc4/detect_missed.m
@@ -0,0 +1,9 @@
+function [out] = detect_missed( position, segment, track)
+%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;
+             0 0 0 0 0 0 0 0 0 ]; % TODO bana 2
+a = track_len(track, segment);
+out = a <= position;
+end
+
diff --git a/Kod/bilbana/yc4/get_aprox_v.m b/Kod/bilbana/yc4/get_aprox_v.m
index 41a3c9e..64fa821 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,last_seg_times)
+function [v] = get_aprox_v(cur_seg, lap, seg_times)
 %GET_APROX_V Tillfällig? Behövs för att testa get_position. Ger
 %medelhastigheten för nuvarande segment på förra varvet.
 %{
@@ -8,8 +8,18 @@ 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];
 
-%v = last_seg_times(cur_seg)/seg_len(cur_seg);
-v = seg_len1(cur_seg)/last_seg_times(cur_seg);
+if cur_seg > 9
+    cur_seg = cur_seg - 9;
+end
+
+while lap > 0
+    lap = lap - 1;
+    if seg_times(lap, cur_seg) ~= 0
+        v = seg_len1(cur_seg)/seg_times(lap, cur_seg);
+        return
+    end
+end
+disp('bara nollor?');
 
 % switch(track)
 %     case 1
diff --git a/Kod/bilbana/yc4/main.m b/Kod/bilbana/yc4/main.m
index 1c613a1..221caea 100644
--- a/Kod/bilbana/yc4/main.m
+++ b/Kod/bilbana/yc4/main.m
@@ -32,6 +32,9 @@ car1.lap_times = [];
 car1.seg_times = [];
 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.approximation = [];
+car1.miss_probability = 0.1;
+%car1.miss_time = uint64(0);
 
 %{
 car2 = struct;
@@ -89,6 +92,11 @@ while 1
     %% READ
     if car1.running == true
 		[car1.new_lap, car1.new_check_point, car1.time] = get_car_position(1);
+        if car1.new_check_point == true && rand < car1.miss_probability && car1.lap >= 4
+            disp('Hoppar över givare');
+            car1.new_check_point = false;
+            beep;
+        end
     end
 	%{
     if car2.running == true
@@ -101,19 +109,43 @@ while 1
         %% CALC POSITION (CAR 1)
         if car1.lap > 1
             last_seg_times1 = car1.seg_times(car1.lap - 1, 1:9);
-            aprox_v = get_aprox_v(car1.segment, last_seg_times1);
+            aprox_v = get_aprox_v(car1.segment + detect_missed( car1.position, car1.segment, 1), car1.lap, car1.seg_times);
             car1.position = get_position(aprox_v, car1.position, t);
+            if detect_missed( car1.position, car1.segment, 1)
+                disp('Miss?');
+                
+                %disp(toc(car1.miss_time));
+                %if car1.miss_time == 0
+                 %   car1.miss_time = tic;
+                %end
+            end
         end
         if car1.new_check_point == true
-            % beep;
-            if car1.lap ~= 0
-                car1.seg_times(car1.lap, car1.segment) = toc(car1.seg_tic);
+            if car1.new_lap == false % choose_position krachar vid nytt varv (seg 10)
+                if car1.lap ~= 0
+                    car1.seg_times(car1.lap, car1.segment) = toc(car1.seg_tic);
+                end
+                car1.segment = car1.segment + 1;
+                car1.seg_tic = tic;
+                if car1.lap > 2 % Säkerhetsmarginal (Bör vara 1?)
+                    disp(car1);
+                    [new_position, seg_plus] = ...
+                            choose_position(car1.position, car1.segment, 1);
+                    if seg_plus ~= 0 && car1.segment == 1
+                        disp('Hoppar över missad givare 1/2');
+                    else
+                        car1.position = new_position;
+                        car1.segment = car1.segment + seg_plus;
+                    end
+                    %car1.miss_time = uint64(0);
+                else
+                    car1.position = car1.seg_len(car1.segment);
+                    %car1.miss_time = uint64(0);
+                end
             end
-            car1.segment = car1.segment + 1;
-            car1.seg_tic = tic;
-            car1.position = car1.seg_len(car1.segment);
         end
         if car1.new_lap == true
+            disp('------------NEW LAP------------')
             if car1.lap == 0
                 % dont save time for first lap
                 car1.segment = 1;
@@ -122,7 +154,7 @@ while 1
                 car1.lap_tic = tic;
                 continue;
             end
-            beep;
+            % beep;
             car1.seg_times(car1.lap, car1.segment) = toc(car1.seg_tic);
             car1.seg_tic = tic;
             car1.lap_times(car1.lap) = toc(car1.lap_tic);
@@ -237,7 +269,7 @@ while 1
                 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;
+                % beep;
             end
             break;
         end