summaryrefslogtreecommitdiffstats
path: root/Kod/bilbana/yc4
diff options
context:
space:
mode:
Diffstat (limited to 'Kod/bilbana/yc4')
-rw-r--r--Kod/bilbana/yc4/choose_position.m41
-rw-r--r--Kod/bilbana/yc4/detect_missed.m9
-rw-r--r--Kod/bilbana/yc4/get_aprox_v.m26
-rw-r--r--Kod/bilbana/yc4/main.m91
4 files changed, 108 insertions, 59 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 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
generated by cgit v1.2.1 (git 2.18.0) at 2025-12-14 16:20:24 +0100