From a170c1524a18fda8cda5a9a322063449a672b36a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gustav=20S=C3=B6rn=C3=A4s?= Date: Tue, 5 Nov 2019 16:31:42 +0100 Subject: Fusk-merge branch 'get_position' --- Kod/bilbana/yc4/main.m | 192 ++++++++++++++++++++++++++++++------------------- 1 file changed, 118 insertions(+), 74 deletions(-) (limited to 'Kod/bilbana/yc4/main.m') diff --git a/Kod/bilbana/yc4/main.m b/Kod/bilbana/yc4/main.m index 2bae58c..85f7be3 100644 --- a/Kod/bilbana/yc4/main.m +++ b/Kod/bilbana/yc4/main.m @@ -18,8 +18,10 @@ initialize_counters(2) config_IOs +load('X:\Git\yc4_2019\Kod\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]; +%} 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,7 +73,7 @@ elseif car2.response == 'M' else car2.running = false; end - +%} %% MAIN LOOP while 1 @@ -82,109 +92,140 @@ while 1 [car1.new_lap, car1.new_check_point, car1.time] = get_car_position(1); end if car2.running == true - [car2.new_lap, car2.new_check_point, car2.time] = get_car_position(2); + % [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.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 + + %{ + TODO 1 -> 2 + + %% 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 + %} %% 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 + %} %% CONTROLLER (CAR 1) if car1.running == true && car1.automatic == false % TODO - end - + end + + %{ %% CONTROLLER (CAR 2) if car2.running == true && car2.automatic == false % TODO 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 @@ -209,10 +250,10 @@ end %% END OF PROGRAM disp(highToc); disp(car1); -disp(car2); +% disp(car2); terminate(1); -terminate(2); +% terminate(2); matlabclient(3); @@ -221,6 +262,9 @@ matlabclient(3); if car1.running == true graphs(car1.lap_times, 13, car1.seg_times, 1); end + +%{ if car2.running == true graphs(car2.lap_times, 13, car2.seg_times, 2); end +%} -- cgit v1.2.1