7 files changed, 169 insertions, 60 deletions

diff --git a/Dokument/Teknisk Dokumentation/spåna på governor.txt b/Dokument/Teknisk Dokumentation/spåna på governor.txt
new file mode 100644
index 0000000..e8cb78b
--- /dev/null
+++ b/Dokument/Teknisk Dokumentation/spåna på governor.txt
@@ -0,0 +1,17 @@
+Funktionen ska utifrån köregenskaper för bil, vald referenstid och, om gemensam målgång aktiverad, skillnad i bilarnas placering och se till att rätt gaspådrag skickas till banan. 
+
+Vi utgår ifrån de redan skrivna bankartorna (bana1.mat och bana2.mat) och multiplicerar dess spänningsvärde med en car_constant. Programmet kommer sedan ihåg hur lång tid varje segment tagit för olika car_constant och gör en modell över hur hög den bör vara för att nå en viss segmentstid. gov_set väljer sedan segmentstider utifrån vald referenstid (och om gemensam målgång aktiverad car_diff).
+
+gov_mem
+Sparar car_constant och tid från varje segment.
+
+gov_calc
+gör en modell över hur lång tid ett segment tar relativt sin car_constant. 
+
+gov_set
+Väljer en tid för varje segment utifrån referenstid, car_diff (om gemensam målgång aktiverad)
+
+Almänna komentarer:
+Kan behövas nån form av ingångshastighet till varje segment så att segmentstidens samband med car_constant tydligare kan ses. Beroende på hur snabbt bilen accelererar kan detta vara helt onödigt.
+Hur ska kallibrerigsvarven hanteras? Antagligen behövs specialkod för första varvet så att bilen inte behöver köra jättelångsamt hela varvet. Kanse går det att gissa på en lite högre car_constant efter bara nåt varv.
+Tänker att övre gräns ej behöver sättas om bankartorna är korrekt gjorda.
\ No newline at end of file
diff --git a/Kod/bilbana/Graph/graphs_test.m b/Kod/bilbana/Graph/graphs_test.m
index ddc3b64..955aeee 100644
--- a/Kod/bilbana/Graph/graphs_test.m
+++ b/Kod/bilbana/Graph/graphs_test.m
@@ -1,3 +1,4 @@
+%{
 %% Data needed
 A = [3.9,1.1,2.2,1.8,1.4,3.9,1.5,3.4,1.4;
      4.2,1.1,2.2,1.8,1.4,3.4,1.5,3.4,1.4;
@@ -11,4 +12,8 @@ car1.seg_times = C;
 car1.lap_times = [14.1,13.8,14.15,13.9,14.1,14];
 ref_lap_time = 14;
 %% Actual test
-graphs(car1.lap_times,ref_lap_time,car1.seg_times,5)
\ No newline at end of file
+graphs(car1.lap_times,ref_lap_time,car1.seg_times,5)
+%}
+position = 2.54;
+segment = 2;
+detect_missed(position,segment,1)
\ No newline at end of file
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 e402af6..d9c4644 100644
--- a/Kod/bilbana/yc4/get_aprox_v.m
+++ b/Kod/bilbana/yc4/get_aprox_v.m
@@ -1,20 +1,26 @@
-function [v] = get_aprox_v(cur_seg,last_seg_times, track)
-%GET_APROX_V Tillfällig? Behövs för att testa get_position. Ger
-%medelhastigheten för nuvarande segment på förra varvet.
+function [v] = get_aprox_v(cur_seg, lap, seg_times, track)
+%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
+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 = seg_len1(cur_seg)/last_seg_times(cur_seg);
+seg_len = (track == 1 : seg_len1 : seg_len2);
 
-switch(track)
-    case 1
-        v = seg_len1(cur_seg)/last_seg_times(cur_seg);
-    case 2
-        v = seg_len2(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_len(cur_seg)/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 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
diff --git a/Kod/scripts/test_givare.m b/Kod/scripts/test_givare.m
new file mode 100644
index 0000000..204d501
--- /dev/null
+++ b/Kod/scripts/test_givare.m
@@ -0,0 +1,38 @@
+disp('Startar bilbanan. Avsluta med q.')
+hf=figure('position',[0 0 eps eps],'menubar','none');
+
+initialize_counters(1)
+initialize_counters(2)
+
+start_race(1)
+start_race(2)
+
+config_IOs
+
+car1 = struct;
+car2 = struct;    
+
+while 1
+    %% PRE-LOOP
+    if strcmp(get(hf,'currentcharacter'),'q')
+        close(hf)
+        break
+    end
+    
+    figure(hf)
+    drawnow
+    
+    %% READ
+    [car1.new_lap, car1.new_check_point, car1.time] = get_car_position(1);
+    [car2.new_lap, car2.new_check_point, car2.time] = get_car_position(2);
+    
+    if car1.new_lap == true || car1.new_check_point == true || car2.new_lap == true || car2.new_check_point == true
+       beep;
+    end
+    
+    pause(0.1)
+end
+
+%%
+terminate(1)
+terminate(2)
\ No newline at end of file