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/do_car.m132
-rw-r--r--Kod/bilbana/yc4/get_aprox_v.m37
-rw-r--r--Kod/bilbana/yc4/get_car_constant.m4
-rw-r--r--Kod/bilbana/yc4/get_new_v.m21
-rw-r--r--Kod/bilbana/yc4/main.m15
7 files changed, 226 insertions, 33 deletions
diff --git a/Kod/bilbana/yc4/choose_position.m b/Kod/bilbana/yc4/choose_position.m
new file mode 100644
index 0000000..d6db2ca
--- /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 2.53 3.05 4.92 7.60 8.84 10.65 14.68 17.76];
+set_pos = [0 2.53 3.05 4.73 7.68 8.98 10.93 14.96 17.57;
+ 0 2.53 3.05 4.92 7.60 8.84 10.65 14.68 17.76];
+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..fb80d1c
--- /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;
+ 2.53 3.05 4.92 7.60 8.84 10.65 14.68 17.76 19.95];
+a = track_len(track, segment);
+out = a <= position;
+end
+
diff --git a/Kod/bilbana/yc4/do_car.m b/Kod/bilbana/yc4/do_car.m
new file mode 100644
index 0000000..504486a
--- /dev/null
+++ b/Kod/bilbana/yc4/do_car.m
@@ -0,0 +1,132 @@
+function [car, stop, display_data] = do_car(car, t, display_data)
+%DO_CAR Ger nya värden till struct car, avgör om koden ska stoppas samt hämtar displaydata.
+%{
+Input/Output:
+car - En struct med data för en viss bil
+ car.num - Vilken bil det är (1 eller 2)
+ car.running - Om bilen körs eller inte
+ car.automatic - Om bilen körs automatiskt eller inte
+ car.segment - Bilens nuvarande segment
+ car.lap - Bilens nuvarande varv
+ car.lap_times - Bilens sparade varvtider (1 x n matris)
+ car.seg_times - Bilens sparade segmentstier (n x m matris)
+ car.position - Bilens nuvarande placering på banan i meter från
+ start/mål
+ car.seg_len - Banans längd från start till givarna (1 x 9 matris)
+ car.map - Tabell med hastighetskoefficienter för alla positioner (.mat
+ fil)
+ car.miss_probability - Sannorlikheten för artificiellt introducerade
+ missade givare
+t - Längden (s) på nuvarande programcykel
+display_data - Buffer med den data som ska skickas till displayen vid nästa
+ anrop
+stop - Huruvida koden ska stoppas eller inte
+%}
+stop = false;
+if car.running == true
+ [car.new_lap, car.new_check_point, car.time] = get_car_position(car.num);
+ if car.new_check_point == true && rand < car.miss_probability && car.lap >= 4
+ disp('Hoppar �ver givare');
+ car.new_check_point = false;
+ beep;
+ end
+end
+
+%% CHECK LAP AND CHECKPOINT
+if car.running == true
+ if car.lap ~= 0
+ if toc(car.seg_tic) > 9.0
+ set_car_speed(1, 0);
+ set_car_speed(2, 0);
+ %disp(strjoin({'Avåkning bil', num2str(car.num)}));
+ disp('J = Ja, N = Nej')
+ car.response = input('Vill du fortsätta? [N] ', 's');
+ if car.response == 'J'
+ car.seg_tic = tic;
+ else
+ stop = true;
+ return;
+ end
+ end
+ end
+ %% CALC POSITION
+ if car.lap > 1
+ % car.last_seg_times = car.seg_times(car.lap - 1, 1:9);
+ aprox_v = get_aprox_v(car.segment + detect_missed(car.position, car.segment, car.num), car.lap, car.seg_times, car.num);
+ car.position = get_position(aprox_v, car.position, t);
+ if detect_missed( car.position, car.segment, car.num)
+ disp('Miss?');
+
+ %disp(toc(car.miss_time));
+ %if car.miss_time == 0
+ % car.miss_time = tic;
+ %end
+ end
+ end
+ if car.new_check_point == true
+ if car.new_lap == false % choose_position krachar vid nytt varv (seg 10)
+ if car.lap ~= 0
+ car.seg_times(car.lap, car.segment) = toc(car.seg_tic);
+ end
+ car.segment = car.segment + 1;
+ car.seg_tic = tic;
+ if car.lap > 2 % S�kerhetsmarginal (B�r vara 1?)
+ disp(car);
+ [new_position, seg_plus] = ...
+ choose_position(car.position, car.segment, car.num);
+ if seg_plus ~= 0 && car.segment == 1
+ disp('Hoppar �ver missad givare 1/2');
+ else
+ car.position = new_position;
+ car.segment = car.segment + seg_plus;
+ end
+ %car.miss_time = uint64(0);
+ else
+ car.position = car.seg_len(car.segment);
+ %car.miss_time = uint64(0);
+ end
+ end
+ end
+ if car.new_lap == true
+ car.new_lap = false;
+ beep;
+ if car.lap == 0
+ % dont save time for first lap
+ car.segment = 1;
+ car.lap = car.lap + 1;
+ car.seg_tic = tic;
+ car.lap_tic = tic;
+ else
+ % beep;
+ car.seg_times(car.lap, car.segment) = toc(car.seg_tic);
+ car.seg_tic = tic;
+ car.lap_times(car.lap) = toc(car.lap_tic);
+ car.lap_tic = tic;
+ car.position = 0;
+
+ display_data = {display_data, put_text(100, 16 + (16 * car.num), 'L', strjoin({num2str(car.lap), get_time_as_string(round(car.lap_times(car.lap) * 1000))}, ' '))};
+
+ car.segment = 1;
+ car.lap = car.lap + 1;
+ end
+ end
+end
+
+%% CALCULATE
+if car.running == true && car.automatic == true
+ car.car_constant = get_car_constant(car.num);
+ car.v = get_new_v(car.position, car.map);
+ car.track_u_constant = get_track_u_constant();
+ car.u = get_new_u(car.v, car.car_constant, car.track_u_constant);
+end
+
+%% CONTROLLER
+if car.running == true && car.automatic == false
+ % set_car_speed(car.num, mult * ((max - get_manual_speed(car.num)) / div));
+end
+
+%% EXECUTE
+if car.running == true && car.automatic == true
+ set_car_speed(car.num, car.u);
+end
+end
diff --git a/Kod/bilbana/yc4/get_aprox_v.m b/Kod/bilbana/yc4/get_aprox_v.m
index 41a3c9e..1867565 100644
--- a/Kod/bilbana/yc4/get_aprox_v.m
+++ b/Kod/bilbana/yc4/get_aprox_v.m
@@ -1,21 +1,32 @@
-function [v] = get_aprox_v(cur_seg,last_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.
+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];
+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);
+% seg_len = [];
-% 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);
-% end
+if track == 1
+ seg_len = seg_len1;
+else
+ seg_len = seg_len2;
end
+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/get_car_constant.m b/Kod/bilbana/yc4/get_car_constant.m
index eb3bd22..e5e9801 100644
--- a/Kod/bilbana/yc4/get_car_constant.m
+++ b/Kod/bilbana/yc4/get_car_constant.m
@@ -13,9 +13,9 @@ precision.
switch(in_pos)
case 1
- car_constant = 1;
+ car_constant = 0.95;
case 2
- car_constant = 1.2;
+ car_constant = 1.2;
end
end
diff --git a/Kod/bilbana/yc4/get_new_v.m b/Kod/bilbana/yc4/get_new_v.m
index 273be4d..e722326 100644
--- a/Kod/bilbana/yc4/get_new_v.m
+++ b/Kod/bilbana/yc4/get_new_v.m
@@ -1,20 +1,21 @@
-function [ new_v ] = get_new_v( old_v, speed_constant, target_diff, car_position_diff, agressiveness )
+function [ new_v ] = get_new_v( position, list)
%GET_NEW_V Hastigheten som bilen ska få nästa cykel.
%{
Utgår ifrån position och ger motsvarande hårdkodade värde på v för
nuvarande sub_segment.
%}
-position = old_v*100; %temp input för position
-list = speed_constant; %temp input för listan Bana1
+position = position*100; %temp input för position
+
+
+
for i = 1:length(list)
- if position > 1960
- new_v = list(length(list),4);
-
- elseif list(i,1) > position
+ if list(i,1) > position
new_v = list((i-1),4);
break
- end
-
-end
+ elseif i == length(list)
+ new_v = list(80,4);
+ break
+ end
end
+
diff --git a/Kod/bilbana/yc4/main.m b/Kod/bilbana/yc4/main.m
index 4733055..cc15f34 100644
--- a/Kod/bilbana/yc4/main.m
+++ b/Kod/bilbana/yc4/main.m
@@ -22,6 +22,7 @@ hf=figure('position', [0 0 eps eps], 'menubar', 'none');
initialize_counters(1)
initialize_counters(2)
+
config_IOs
load('bilbana\files\Bana1.mat')
@@ -31,6 +32,7 @@ start_race(1)
start_race(2)
car1 = struct;
+car1.num = 1;
car1.running = false;
car1.automatic = true;
car1.segment = 1;
@@ -43,10 +45,9 @@ car1.seg_len = [0.0 2.53 3.05 4.73 7.68 8.98 10.93 14.69 17.57];
car1.map = Bana1;
car1.approximation = [];
car1.miss_probability = 0.0;
-car1.num = 1;
-
car2 = struct;
+car2.num = 2;
car2.running = false;
car2.automatic = true;
car2.segment = 1;
@@ -58,8 +59,6 @@ car2.position = 0;
car2.seg_len = [0.0 2.53 3.05 4.92 7.60 8.84 10.65 14.68 17.76];
car2.map = Bana2;
car2.miss_probability = 0.0;
-car2.num = 2;
-
highToc = 0;
@@ -105,14 +104,14 @@ while 1
[car1, car1.stop, display_data] = do_car(car1, t, display_data);
[car2, car2.stop, display_data] = do_car(car2, t, display_data);
- if car1.stop == true
+ if car1.stop == true
disp('stopped by car 1');
break;
- end
- if car2.stop == true
+ end
+ if car2.stop == true
disp('stopped by car 2');
break;
- end
+ end
%% END OF LOOP
while 1 %Whileloop med paus som k�rs till pausen �verskridit 0.07 sekunder
generated by cgit v1.2.1 (git 2.18.0) at 2025-12-14 04:55:05 +0100