Merge branch 'position_track2' into master

1 files changed, 42 insertions, 49 deletions

diff --git a/Kod/bilbana/yc4/main.m b/Kod/bilbana/yc4/main.m
index b3c2b56..66e8db3 100644
--- a/Kod/bilbana/yc4/main.m
+++ b/Kod/bilbana/yc4/main.m
@@ -34,6 +34,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;
@@ -92,6 +95,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,23 +109,46 @@ while 1
     
     %% CHECK LAP AND CHECKPOINT (CAR 1)
     if car1.running == true
-        
         %% 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, 1);
+            aprox_v = get_aprox_v(car1.segment + detect_missed( car1.position, car1.segment, 1), car1.lap, car1.seg_times, 1);
             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;
@@ -126,7 +157,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);
@@ -143,45 +174,7 @@ while 1
     
     
     %% CHECK LAP AND CHECKPOINT (CAR 2)
-    if car2.running == true
-        
-        %% CALC POSITION (CAR 2
-       if car2.lap > 1
-           last_seg_times2 = car2.seg_times(car2.lap - 1, 1:9);
-           aprox_v = get_aprox_v(car2.segment, last_seg_times2, 2);
-           car2.position = get_position(aprox_v, car2.position, t);
-       end
-        if car2.new_check_point == true
-            % beep;
-            if car2.lap ~= 0
-                car2.seg_times(car2.lap, car2.segment) = toc(car2.seg_tic);
-            end
-            car2.segment = car2.segment + 1;
-            car2.seg_tic = tic;
-            car2.position = car2.seg_len(car2.segment);
-        end
-        if car2.new_lap == true
-            if car2.lap == 0
-                % dont save time for first lap
-                car2.segment = 1;
-                car2.lap = car2.lap + 1;
-                car2.seg_tic = tic;
-                car2.lap_tic = tic;
-                continue;
-            end
-            beep;
-            car2.seg_times(car2.lap, car2.segment) = toc(car2.seg_tic);
-            car2.seg_tic = tic;
-            car2.lap_times(car2.lap) = toc(car2.lap_tic);
-            car2.lap_tic = tic;
-            car2.position = 0;
-
-            display_data = {display_data, put_text(100, 32, 'L', strjoin({num2str(car2.lap), get_time_as_string(round(car2.lap_times(car2.lap) * 1000))}, ' '))};
-
-            car2.segment = 1;
-            car2.lap = car2.lap + 1;
-        end
-    end
+    % TODO copy from car1
     
 
 	%% CALCULATE (CAR 1)
@@ -241,7 +234,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