From ca01e2e6e69fd702ed2f4cf56d6323593bea3278 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gustav=20S=C3=B6rn=C3=A4s?= Date: Tue, 12 Nov 2019 12:34:14 +0100 Subject: Add test IO --- Kod/bilbana/yc4/display/get_package.m | 10 ++------ Kod/bilbana/yc4/display/get_response.m | 34 ++++++++++++++++++++++++++ Kod/bilbana/yc4/display/get_smallpackage.m | 8 ++++++ Kod/bilbana/yc4/display/send_data_to_display.m | 4 +++ 4 files changed, 48 insertions(+), 8 deletions(-) create mode 100644 Kod/bilbana/yc4/display/get_response.m create mode 100644 Kod/bilbana/yc4/display/get_smallpackage.m (limited to 'Kod/bilbana') diff --git a/Kod/bilbana/yc4/display/get_package.m b/Kod/bilbana/yc4/display/get_package.m index 7c3a01e..efaf311 100644 --- a/Kod/bilbana/yc4/display/get_package.m +++ b/Kod/bilbana/yc4/display/get_package.m @@ -1,10 +1,4 @@ -function [pkg] = get_package(code, args) - DC1 = 17; +function pkg = get_package(code, args) ESC = 27; - - data = [ESC, double(code), args]; - len = length(data); - initStr = [DC1, len, data]; - bcc = mod(sum(initStr), 256); - pkg = [initStr, bcc]; + pkg = [ESC, double(code), args]; end diff --git a/Kod/bilbana/yc4/display/get_response.m b/Kod/bilbana/yc4/display/get_response.m new file mode 100644 index 0000000..9714a43 --- /dev/null +++ b/Kod/bilbana/yc4/display/get_response.m @@ -0,0 +1,34 @@ +function [ack, start_code, data] get_response(display_data) +% GET RESPONSE +% In-depth explanation +% [flag, display_data] = matlabclient(2) + +ack = false; +start_code = ''; +data = []; + +len = -1; + +if display_data[0] == 6 + ack = true; +else + return +end + +display_data[0] = []; + +if display_data[0] == 17 + start_code = 'DC1'; +elseif display_data[0] == 18 + start_code = 'DC2'; +end + +display_data[0] = []; + +len = display_data[0]; + +while len > 0 + +end + +end diff --git a/Kod/bilbana/yc4/display/get_smallpackage.m b/Kod/bilbana/yc4/display/get_smallpackage.m new file mode 100644 index 0000000..1c0835c --- /dev/null +++ b/Kod/bilbana/yc4/display/get_smallpackage.m @@ -0,0 +1,8 @@ +function pkg = get_smallpackage(data) + DC1 = 17; + + len = length(data); + initStr = [DC1, len, data]; + bcc = mod(sum(initStr), 256); + pkg = [initStr, bcc]; +end diff --git a/Kod/bilbana/yc4/display/send_data_to_display.m b/Kod/bilbana/yc4/display/send_data_to_display.m index c5d70c7..50af84f 100644 --- a/Kod/bilbana/yc4/display/send_data_to_display.m +++ b/Kod/bilbana/yc4/display/send_data_to_display.m @@ -20,6 +20,10 @@ function [] = send_data_to_display() %% SEND DATA % disp('sending data'); % disp(display_data) + + data = []; + + matlabclient(1, display_data{1}); last_send = clock; display_data(1) = []; -- cgit v1.2.1 From f735459829046e9e2a9911d8cf31546a5bebefee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gustav=20S=C3=B6rn=C3=A4s?= Date: Tue, 12 Nov 2019 12:34:58 +0100 Subject: fix func name --- Kod/bilbana/yc4/display/define_touch_switch.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'Kod/bilbana') diff --git a/Kod/bilbana/yc4/display/define_touch_switch.m b/Kod/bilbana/yc4/display/define_touch_switch.m index 47d3cd9..4dfc764 100644 --- a/Kod/bilbana/yc4/display/define_touch_switch.m +++ b/Kod/bilbana/yc4/display/define_touch_switch.m @@ -1,4 +1,4 @@ -function pkg = define_touch_key(x1, y1, x2, y2, down_code, up_code, just, text) +function pkg = define_touch_switch(x1, y1, x2, y2, down_code, up_code, just, text) arg_location = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; arg_text = double(strjoin({just; text}, '')); arg_null = 0; -- cgit v1.2.1 From 83c1ee0a3f2d57d887d38fafb74f4967271c4a84 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?David=20Thor=C3=A9n?= Date: Wed, 13 Nov 2019 12:32:01 +0100 Subject: Add function to convert bytes -> num --- Kod/bilbana/yc4/display/get_num.m | 3 + Kod/bilbana/yc4/main.m | 181 +++++++------------------------------- 2 files changed, 34 insertions(+), 150 deletions(-) create mode 100644 Kod/bilbana/yc4/display/get_num.m (limited to 'Kod/bilbana') diff --git a/Kod/bilbana/yc4/display/get_num.m b/Kod/bilbana/yc4/display/get_num.m new file mode 100644 index 0000000..e90e0c7 --- /dev/null +++ b/Kod/bilbana/yc4/display/get_num.m @@ -0,0 +1,3 @@ +function num = get_num(bytes) +num = bytes(1) + bytes(2) * 256; +end \ No newline at end of file diff --git a/Kod/bilbana/yc4/main.m b/Kod/bilbana/yc4/main.m index 1c613a1..12f9dd1 100644 --- a/Kod/bilbana/yc4/main.m +++ b/Kod/bilbana/yc4/main.m @@ -11,17 +11,19 @@ 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) + config_IOs load('bilbana\files\Bana1.mat') +load('bilbana\files\Bana2.mat') start_race(1) -% start_race(2) +start_race(2) car1 = struct; car1.running = false; @@ -30,10 +32,15 @@ car1.segment = 1; car1.lap = 0; car1.lap_times = []; car1.seg_times = []; +car1.last_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.map = Bana1; +car1.approximation = []; +car1.miss_probability = 0.0; +car1.num = 1; + -%{ car2 = struct; car2.running = false; car2.automatic = true; @@ -41,9 +48,13 @@ car2.segment = 1; car2.lap = 0; car2.lap_times = []; car2.seg_times = []; +car2.last_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]; -%} +car2.map = Bana2; +car2.miss_probability = 0.0; +car2.num = 2; + highToc = 0; @@ -61,7 +72,7 @@ else car1.running = false; end -%{ + car2.response = input('Vill du köra bil 2? [N] ', 's'); if car2.response == 'J' car2.running = true; @@ -72,7 +83,7 @@ elseif car2.response == 'M' else car2.running = false; end -%} + %% MAIN LOOP while 1 @@ -86,146 +97,18 @@ while 1 figure(hf) drawnow - %% READ - if car1.running == true - [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); - end - %} - - %% 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); - car1.position = get_position(aprox_v, car1.position, t); - 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 - - %{ - %% CHECK LAP AND CHECKPOINT (CAR 2) - if car2.running == true - %% CALC POSITION (CAR 2) - if car2.lap ~= 0 - if car2.lap > 1 - last_seg_times1 = car2.seg_times(car2.lap - 1, 1:9); - aprox_v = get_aprox_v(car2.segment, last_seg_times2); %TODO ?? - car2.position = get_position(aprox_v, car2.position, delta_t); - end - 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 - %} - - %% CALCULATE (CAR 1) - if car1.running == true && car1.automatic == true - car1.car_constant = get_car_constant(1); - 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 - - %{ - %% 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 - %} - - % TEMP VARIABLES FOR MANUAL CONTROL - mult = 100; - max = 55; - div = 55; + [car1, car1.stop, display_data] = do_car(car1, t, display_data); + [car2, car2.stop, display_data] = do_car(car2, t, display_data); - %% CONTROLLER (CAR 1) - if car1.running == true && car1.automatic == false - set_car_speed(1, mult * ((max - get_manual_speed(1)) / div)); + if car1.stop == true + disp('stopped by car 1'); + break; end - - %{ - %% CONTROLLER (CAR 2) - if car2.running == true && car2.automatic == false - set_car_speed(2, _mult * ((_max - get_manual_speed(2)) / _div)); + if car2.stop == true + disp('stopped by car 2'); + break; 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 @@ -237,33 +120,31 @@ 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 end - send_data_to_display(); end %% END OF PROGRAM 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.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 -%} -- cgit v1.2.1 From 2f0a6f442b8103a95839f0e37d5689ca9cf0fe71 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?David=20Thor=C3=A9n?= Date: Wed, 13 Nov 2019 12:32:46 +0100 Subject: Working communication from display --- Kod/bilbana/yc4/display/get_response.m | 47 ++++++++++++++++++++++----------- Kod/bilbana/yc4/main.m | 48 +++++++++++++++++++++++++--------- 2 files changed, 67 insertions(+), 28 deletions(-) (limited to 'Kod/bilbana') diff --git a/Kod/bilbana/yc4/display/get_response.m b/Kod/bilbana/yc4/display/get_response.m index 9714a43..1e3e6af 100644 --- a/Kod/bilbana/yc4/display/get_response.m +++ b/Kod/bilbana/yc4/display/get_response.m @@ -1,34 +1,51 @@ -function [ack, start_code, data] get_response(display_data) +function [ack, start_code, responses] = get_response(display_data) % GET RESPONSE % In-depth explanation % [flag, display_data] = matlabclient(2) ack = false; start_code = ''; -data = []; +bcc = display_data(length(display_data)); +responses = []; -len = -1; +pointer = 1; -if display_data[0] == 6 +if display_data(pointer) == 6 ack = true; else return end -display_data[0] = []; +if pointer > length(display_data) + return +end -if display_data[0] == 17 +pointer = pointer + 1; +if display_data(1) == 17 start_code = 'DC1'; -elseif display_data[0] == 18 +elseif display_data(1) == 18 start_code = 'DC2'; end -display_data[0] = []; - -len = display_data[0]; - -while len > 0 - -end - +pointer = pointer + 1; +% total length + +data = struct; +while pointer < length(display_data) - 1 % last value is bcc + pointer = pointer + 1; + if display_data(pointer) ~= 27 + % TODO: no ESC? + end + pointer = pointer + 1; + data.id = char(display_data(pointer)); + + pointer = pointer + 1; + data.length = display_data(pointer); + if data.id == 'A' + pointer = pointer + 1; + data.data = display_data(pointer); + else + pointer = pointer + data.length; + end + responses = [responses, data]; end diff --git a/Kod/bilbana/yc4/main.m b/Kod/bilbana/yc4/main.m index 12f9dd1..4733055 100644 --- a/Kod/bilbana/yc4/main.m +++ b/Kod/bilbana/yc4/main.m @@ -5,10 +5,16 @@ cd display/ClientServerApp/Release !startServer cd ../../.. -global display_data; -display_data = {}; -display_data = {display_data clear_display()}; -pause(1); +display = struct; +display.data = [clear_display()]; +display.out = 0; +display.shm = 0; +display.shm_interp = struct; +display.shm_interp.ack = 0; +display.shm_interp.start_code = ''; +display.shm_interp.data = []; +display.last_send = tic; +display.last_request = tic; disp('Startar bilbanan. Avsluta med q.') hf=figure('position', [0 0 eps eps], 'menubar', 'none'); @@ -16,7 +22,6 @@ hf=figure('position', [0 0 eps eps], 'menubar', 'none'); initialize_counters(1) initialize_counters(2) - config_IOs load('bilbana\files\Bana1.mat') @@ -100,21 +105,38 @@ 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 - - %% DISPLAY + end %% END OF LOOP while 1 %Whileloop med paus som k�rs till pausen �verskridit 0.07 sekunder - pause(0.001); + % DISPLAY + display.send_delay = tic; + if toc(display.last_send) > display.send_interval + % send all queued data + if ~isempty(display.data) + [display.out] = matlabclient(1, get_smallpackage(display.data)); + display.data = []; + end + display.last_send = tic; + + % read internal mem from last send + [display.out, display.shm] = matlabclient(2); + [display.shm_interp.ack, display.shm_interp.start_code, display.shm_interp.data] = get_response(display.shm); + + % request internal mem + matlabclient(1, hex2dec(['12'; '01'; '53'; '66'])); + end + disp(strjoin{'display took additional ', num2str(toc(display.send_delay))}); + % ACTUAL END OF LOOP 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 @@ -124,8 +146,8 @@ while 1 end break; end + pause(0.001); end - send_data_to_display(); end %% END OF PROGRAM -- cgit v1.2.1