summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Kod/bilbana/yc4/choose_position.m16
-rw-r--r--Kod/bilbana/yc4/detect_missed.m2
-rw-r--r--Kod/bilbana/yc4/get_aprox_v.m10
-rw-r--r--Kod/bilbana/yc4/get_car_constant.m4
-rw-r--r--Kod/bilbana/yc4/main.m112
5 files changed, 114 insertions, 30 deletions
diff --git a/Kod/bilbana/yc4/choose_position.m b/Kod/bilbana/yc4/choose_position.m
index 39be6e4..d6db2ca 100644
--- a/Kod/bilbana/yc4/choose_position.m
+++ b/Kod/bilbana/yc4/choose_position.m
@@ -5,19 +5,19 @@ function [new_position, seg_plus] = choose_position(position,segment, track)
% 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];
+ 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 0 0 0 0 0 0 0 0];
+ 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);
+ diff = abs(track_len(track, i) - pos_c);
if i == 1
- near = [i,diff];
+ near = [i, diff];
else
if diff < near(2)
- near = [i,diff];
+ near = [i, diff];
end
end
end
@@ -29,10 +29,10 @@ if near(1) == segment
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);
+ 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
+ new_position = set_pos(track, segment); % ineff borde kombineras
seg_plus = 0;
disp('In right segment ich');
end
diff --git a/Kod/bilbana/yc4/detect_missed.m b/Kod/bilbana/yc4/detect_missed.m
index caad2ac..fb80d1c 100644
--- a/Kod/bilbana/yc4/detect_missed.m
+++ b/Kod/bilbana/yc4/detect_missed.m
@@ -2,7 +2,7 @@ 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
+ 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/get_aprox_v.m b/Kod/bilbana/yc4/get_aprox_v.m
index d9c4644..1867565 100644
--- a/Kod/bilbana/yc4/get_aprox_v.m
+++ b/Kod/bilbana/yc4/get_aprox_v.m
@@ -8,7 +8,13 @@ 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_len = (track == 1 : seg_len1 : seg_len2);
+% seg_len = [];
+
+if track == 1
+ seg_len = seg_len1;
+else
+ seg_len = seg_len2;
+end
if cur_seg > 9
cur_seg = cur_seg - 9;
@@ -17,7 +23,7 @@ end
while lap > 0
lap = lap - 1;
if seg_times(lap, cur_seg) ~= 0
- v = seg_len(cur_seg)/seg_times(lap, cur_seg);
+ v = seg_len(cur_seg) / seg_times(lap, cur_seg);
return
end
end
diff --git a/Kod/bilbana/yc4/get_car_constant.m b/Kod/bilbana/yc4/get_car_constant.m
index eb3bd22..5513721 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 = 0.8;
end
end
diff --git a/Kod/bilbana/yc4/main.m b/Kod/bilbana/yc4/main.m
index 66e8db3..4000d02 100644
--- a/Kod/bilbana/yc4/main.m
+++ b/Kod/bilbana/yc4/main.m
@@ -11,7 +11,7 @@ display_data = {display_data clear_display()};
pause(1);
disp('Startar bilbanan. Avsluta med q.')
-hf=figure('position',[0 0 eps eps],'menubar','none');
+hf=figure('position', [0 0 eps eps], 'menubar', 'none');
initialize_counters(1)
initialize_counters(2)
@@ -106,13 +106,26 @@ while 1
[car2.new_lap, car2.new_check_point, car2.time] = get_car_position(2);
end
-
%% CHECK LAP AND CHECKPOINT (CAR 1)
if car1.running == true
+ if car1.lap ~= 0
+ if toc(car1.seg_tic) > 9.0
+ set_car_speed(1, 0);
+ set_car_speed(2, 0);
+ disp('Avåkning bil 1');
+ disp('J = Ja, N = Nej')
+ car1.response = input('Vill du fortsätta? [N] ', 's');
+ if car1.response == 'J'
+ car1.seg_tic = tic;
+ else
+ break;
+ end
+ end
+ end
%% 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 + detect_missed( car1.position, car1.segment, 1), car1.lap, car1.seg_times, 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?');
@@ -170,12 +183,83 @@ while 1
car1.lap = car1.lap + 1;
end
end
-
-
%% CHECK LAP AND CHECKPOINT (CAR 2)
- % TODO copy from car1
-
+ if car2.running == true
+ if car2.lap ~= 0
+ if toc(car2.seg_tic) > 9.0
+ set_car_speed(1, 0);
+ set_car_speed(2, 0);
+ disp('Avåkning bil 2');
+ disp('J = Ja, N = Nej')
+ car2.response = input('Vill du fortsätta? [N] ', 's');
+ if car2.response == 'J'
+ car2.seg_tic = tic;
+ else
+ break;
+ end
+ end
+ end
+ if car2.lap > 1
+ last_seg_times2 = car2.seg_times(car2.lap - 1, 1:9);
+ aprox_v = get_aprox_v(car2.segment + detect_missed(car2.position, car2.segment, 2), car2.lap, car2.seg_times, 2);
+ car2.position = get_position(aprox_v, car2.position, t);
+ if detect_missed( car2.position, car2.segment, 2)
+ disp('Miss?');
+
+ %disp(toc(car2.miss_time));
+ %if car2.miss_time == 0
+ % car2.miss_time = tic;
+ %end
+ end
+ end
+ if car2.new_check_point == true
+ if car2.new_lap == false % choose_position krachar vid nytt varv (seg 10)
+ 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;
+ disp(car2);
+ if car2.lap > 2 % S�kerhetsmarginal (B�r vara 1?)
+ [new_position, seg_plus] = ...
+ choose_position(car2.position, car2.segment, 2);
+ if seg_plus ~= 0 && car2.segment == 1
+ disp('Hoppar �ver missad givare 1/2');
+ else
+ car2.position = new_position;
+ car2.segment = car2.segment + seg_plus;
+ end
+ %car2.miss_time = uint64(0);
+ else
+ car2.position = car2.seg_len(car2.segment);
+ %car2.miss_time = uint64(0);
+ end
+ end
+ end
+ if car2.new_lap == true
+ disp('------------NEW LAP------------')
+ 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, 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
%% CALCULATE (CAR 1)
if car1.running == true && car1.automatic == true
@@ -185,15 +269,13 @@ while 1
car1.u = get_new_u(car1.v, car1.car_constant, car1.track_u_constant);
end
-
%% CALCULATE (CAR 2)
if car2.running == true && car2.automatic == true
car2.car_constant = get_car_constant(2);
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
-
+ end
% TEMP VARIABLES FOR MANUAL CONTROL
mult = 100;
@@ -205,24 +287,20 @@ while 1
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
-
+ end
%% EXECUTE
if car1.running == true && car1.automatic == true
set_car_speed(1, car1.u);
end
-
if car2.running == true && car2.automatic == true
set_car_speed(2, car2.u);
end
-
%% DISPLAY
%% END OF LOOP
@@ -256,10 +334,10 @@ matlabclient(3);
%% DISPLAY GRAPHS
if car1.running == true
- graphs(car1.lap_times, 13.00, car1.seg_times, 1);
+ graphs(car1.lap_times, 13, car1.seg_times, 1);
end
if car2.running == true
graphs(car2.lap_times, 13, car2.seg_times, 2);
-end \ No newline at end of file
+end
generated by cgit v1.2.1 (git 2.18.0) at 2025-12-14 06:18:25 +0100