summaryrefslogtreecommitdiffstats
path: root/Kod/bilbana/yc4
diff options
context:
space:
mode:
Diffstat (limited to 'Kod/bilbana/yc4')
-rw-r--r--Kod/bilbana/yc4/get_aprox_v.m21
-rw-r--r--Kod/bilbana/yc4/get_new_v.m31
-rw-r--r--Kod/bilbana/yc4/get_position.m15
-rw-r--r--Kod/bilbana/yc4/main.m222
4 files changed, 188 insertions, 101 deletions
diff --git a/Kod/bilbana/yc4/get_aprox_v.m b/Kod/bilbana/yc4/get_aprox_v.m
new file mode 100644
index 0000000..41a3c9e
--- /dev/null
+++ b/Kod/bilbana/yc4/get_aprox_v.m
@@ -0,0 +1,21 @@
+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.
+%{
+cur_seg: Nuvarande segment
+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);
+
+% 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
+end
+
diff --git a/Kod/bilbana/yc4/get_new_v.m b/Kod/bilbana/yc4/get_new_v.m
index 26bf175..273be4d 100644
--- a/Kod/bilbana/yc4/get_new_v.m
+++ b/Kod/bilbana/yc4/get_new_v.m
@@ -1,21 +1,20 @@
function [ new_v ] = get_new_v( old_v, speed_constant, target_diff, car_position_diff, agressiveness )
%GET_NEW_V Hastigheten som bilen ska få nästa cykel.
%{
-Tar förra cykelns hastighet (old_ v) och lägger till eller drar
-av beroende på hur långt ifrån målet bilarna ligger (target_diff) och,
-om gemensam målgång är aktiverad, hur långt ifrån varandra bilarna är
-(car_position_diff). Beror också på agressiveness; högre agressiveness ger
-större skillnad mellan new_v och old_v medan ett lågt värde gör att new_v
-inte ändras särskilt mycket. new_v används sedan för att sätta new_u.
-Högre new_v ger högre new_u och lägre new_v ger lägre_u.
+Utgår ifrån position och ger motsvarande hårdkodade värde på v för
+nuvarande sub_segment.
%}
-
-%{
-GET_NEW_V:
-new_v: Den hastighet som bilen ska få nästa cykel.
-%}
-
-v = [35, 25, 30, 30, 40, 50, 40, 50, 35];
-new_v = v(old_v);
+position = old_v*100; %temp input för position
+list = speed_constant; %temp input för listan Bana1
+for i = 1:length(list)
+
+ if position > 1960
+ new_v = list(length(list),4);
+
+ elseif list(i,1) > position
+ new_v = list((i-1),4);
+ break
+ end
+
+end
end
-
diff --git a/Kod/bilbana/yc4/get_position.m b/Kod/bilbana/yc4/get_position.m
index 9f70347..72fa866 100644
--- a/Kod/bilbana/yc4/get_position.m
+++ b/Kod/bilbana/yc4/get_position.m
@@ -1,6 +1,13 @@
-function [ output_args ] = get_position( v_prev_cycle, prev_p, delta_t )
-%UNTITLED Summary of this function goes here
-% Detailed explanation goes here
-
+function [ position ] = get_position( aprox_v, prev_p, delta_t)
+%GET_POSITION: Uppskattar position utifrån förra positionen och hastigheten
+%{
+Använder s = v*t för att beräkna skillnaden i strecka sedan förra cykeln.
+v är just nu medelhastigheten för nuvarande segment förra cykeln.
+dt är just nu samma tic toc som checkar att cykeln inte var mer än 0.1 s.
+%}
+v = aprox_v;
+dt = delta_t;
+dp = v*dt;
+position = prev_p + dp;
end
diff --git a/Kod/bilbana/yc4/main.m b/Kod/bilbana/yc4/main.m
index 8acf859..5c089c9 100644
--- a/Kod/bilbana/yc4/main.m
+++ b/Kod/bilbana/yc4/main.m
@@ -18,8 +18,10 @@ initialize_counters(2)
config_IOs
+load('bilbana\files\Bana1.mat')
+
start_race(1)
-start_race(2)
+% start_race(2)
car1 = struct;
car1.running = false;
@@ -28,7 +30,10 @@ car1.segment = 1;
car1.lap = 0;
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];
+%{
car2 = struct;
car2.running = false;
car2.automatic = true;
@@ -36,8 +41,12 @@ car2.segment = 1;
car2.lap = 0;
car2.lap_times = [];
car2.seg_times = [];
+car2.position = 0;
+car2.seg_len = [0.0 2.53 3.05 4.92 7.60 8.84 10.65 14.68 17.76];
+%}
-tocs = [];
+highToc = 0;
+delta_t = 0;
%% ASK ACTIVE CARS
disp('J = Ja (automatiskt), M = Ja (manuellt), N = Nej');
@@ -53,6 +62,7 @@ else
car1.running = false;
end
+%{
car2.response = input('Vill du köra bil 2? [N] ', 's');
if car2.response == 'J'
car2.running = true;
@@ -63,11 +73,11 @@ elseif car2.response == 'M'
else
car2.running = false;
end
-
+%}
%% MAIN LOOP
while 1
- tic;
+ readTime = tic;
%% PRE-LOOP
if strcmp(get(hf,'currentcharacter'),'q')
close(hf)
@@ -81,137 +91,187 @@ while 1
if car1.running == true
[car1.new_lap, car1.new_check_point, car1.time] = get_car_position(1);
end
- if car2.running == true
+ %{
+ if car2.running == true
[car2.new_lap, car2.new_check_point, car2.time] = get_car_position(2);
end
+ %}
+
+ %% CHECK LAP AND CHECKPOINT (CAR 1)
+ if car1.running == true
+
+ %% CALC POSITION (CAR 1)
+ if car1.lap ~= 0
+ 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);
+ car1.position = get_position(aprox_v, car1.position, delta_t)
+ end
+ end
+ if car1.new_check_point == true
+ % beep;
+ 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;
+ car1.position = car1.seg_len(car1.segment);
+ end
+ if car1.new_lap == true
+ if car1.lap == 0
+ % dont save time for first lap
+ car1.segment = 1;
+ car1.lap = car1.lap + 1;
+ car1.seg_tic = tic;
+ car1.lap_tic = tic;
+ continue;
+ end
+ 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);
+ car1.lap_tic = tic;
+ car1.position = 0;
+
+ display_data = {display_data, put_text(100, 32, 'L', strjoin({num2str(car1.lap), get_time_as_string(round(car1.lap_times(car1.lap) * 1000))}, ' '))};
+
+ car1.segment = 1;
+ car1.lap = car1.lap + 1;
+ end
+ end
+
+ %{
+ TODO 1 -> 2
%% CHECK LAP AND CHECKPOINT (CAR 1)
if car1.running == true
- if car1.new_check_point == true
- % beep;
- 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;
- end
- if car1.new_lap == true
- if car1.lap == 0
- % dont save time for first lap
- car1.segment = 1;
- car1.lap = car1.lap + 1;
- car1.seg_tic = tic;
- car1.lap_tic = tic;
- continue;
- end
- 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);
- car1.lap_tic = tic;
-
- display_data = {display_data, put_text(100, 32, 'L', strjoin({num2str(car1.lap), get_time_as_string(round(car1.lap_times(car1.lap) * 1000))}, ' '))};
-
- car1.segment = 1;
- car1.lap = car1.lap + 1;
- end
- end
+
+ %% CALC POSITION (CAR 1)
+ if car1.lap ~= 0
+ 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);
+ car1.position = get_position(aprox_v, car1.position, delta_t);
+ end
+ end
+ if car1.new_check_point == true
+ % beep;
+ 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;
+ car1.position = car1.seg_len(car1.segment);
+ end
+ if car1.new_lap == true
+ if car1.lap == 0
+ % dont save time for first lap
+ car1.segment = 1;
+ car1.lap = car1.lap + 1;
+ car1.seg_tic = tic;
+ car1.lap_tic = tic;
+ continue;
+ end
+ 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);
+ car1.lap_tic = tic;
+ car1.position = 0;
- %% CHECK LAP AND CHECKPOINT (CAR 2)
- if car2.running == true
- 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;
- 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;
- disp('continuing');
- 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;
-
- display_data = {display_data, put_text(100, 48, '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
+ display_data = {display_data, put_text(100, 32, 'L', strjoin({num2str(car1.lap), get_time_as_string(round(car1.lap_times(car1.lap) * 1000))}, ' '))};
+
+ car1.segment = 1;
+ car1.lap = car1.lap + 1;
+ end
+ end
+ %}
%% CALCULATE (CAR 1)
if car1.running == true && car1.automatic == true
car1.car_constant = get_car_constant(1);
- car1.v = get_new_v(car1.segment);
+ car1.v = get_new_v(car1.position, Bana1);
car1.track_u_constant = get_track_u_constant();
car1.u = get_new_u(car1.v, car1.car_constant, car1.track_u_constant);
- end
+ end
+ %{
%% CALCULATE (CAR 2)
if car2.running == true && car2.automatic == true
car2.car_constant = get_car_constant(2);
- car2.v = get_new_v(car2.segment);
+ car2.v = get_new_v(car2.position, Bana2);
car2.track_u_constant = get_track_u_constant();
car2.u = get_new_u(car2.v, car2.car_constant, car2.track_u_constant);
end
+ %}
% TEMP VARIABLES FOR MANUAL CONTROL
- _mult = 100;
- _max = 55;
- _div = 55;
+ mult = 100;
+ max = 55;
+ div = 55;
%% CONTROLLER (CAR 1)
if car1.running == true && car1.automatic == false
- set_car_speed(1, _mult * ((_max - get_manual_speed(1)) / _div));
+ set_car_speed(1, mult * ((max - get_manual_speed(1)) / div));
end
+ %{
%% CONTROLLER (CAR 2)
if car2.running == true && car2.automatic == false
set_car_speed(2, _mult * ((_max - get_manual_speed(2)) / _div));
end
+ %}
%% EXECUTE
if car1.running == true && car1.automatic == true
set_car_speed(1, car1.u);
end
- if car2.running == true && car2.automatic == true
+
+ %{
+ if car2.running == true && car2.automatic == true
set_car_speed(2, car2.u);
end
+ %}
+
%% DISPLAY
%% END OF LOOP
- pause(0.1)
- tocs(end + 1) = toc;
+ while 1 %Whileloop med paus som k�rs till pausen �verskridit 0.07 sekunder
+ pause(0.001);
+ t = toc(readTime);
+ if t > 0.07
+ if t > highToc
+ 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;
+ end
+ break;
+ end
+ end
+
+
send_data_to_display();
end
%% END OF PROGRAM
-disp(tocs);
+disp(highToc);
disp(car1);
-disp(car2);
+% disp(car2);
terminate(1);
-terminate(2);
+% terminate(2);
matlabclient(3);
%% DISPLAY GRAPHS
if car1.running == true
- graphs(car1.lap_times, 13, car1.seg_times, 1);
+ graphs(car1.lap_times, 14.25, car1.seg_times, 1);
end
+
+%{
if car2.running == true
graphs(car2.lap_times, 13, car2.seg_times, 2);
end
+%}
generated by cgit v1.2.1 (git 2.18.0) at 2025-12-14 13:31:20 +0100