diff options
Diffstat (limited to 'Kod/bilbana/yc4')
47 files changed, 513 insertions, 244 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/display/clear_display.m b/Kod/bilbana/yc4/display/clear_display.m index 32fdac1..45f1680 100644 --- a/Kod/bilbana/yc4/display/clear_display.m +++ b/Kod/bilbana/yc4/display/clear_display.m @@ -1,3 +1,3 @@ function pkg = clear_display() - pkg = get_package('DL', []) +pkg = get_package('DL', []); end diff --git a/Kod/bilbana/yc4/display/continue_line.m b/Kod/bilbana/yc4/display/continue_line.m index 9734201..99b4074 100644 --- a/Kod/bilbana/yc4/display/continue_line.m +++ b/Kod/bilbana/yc4/display/continue_line.m @@ -1,5 +1,4 @@ function pkg = continue_line(x2, y2) - arg = [get_bytes(x2), get_bytes(y2)]; - - pkg = get_package('GW', arg) +arg = [get_bytes(x2), get_bytes(y2)]; +pkg = get_package('GW', arg) end diff --git a/Kod/bilbana/yc4/display/define_flashing_area.m b/Kod/bilbana/yc4/display/define_flashing_area.m index 11d6cf3..3e581ea 100644 --- a/Kod/bilbana/yc4/display/define_flashing_area.m +++ b/Kod/bilbana/yc4/display/define_flashing_area.m @@ -1,5 +1,4 @@ function pkg = define_flashing_area(x1, y1, x2, y2) - arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; - - pkg = get_package('QI', arg); +arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; +pkg = get_package('QI', arg); end diff --git a/Kod/bilbana/yc4/display/define_flashing_area_with_pattern.m b/Kod/bilbana/yc4/display/define_flashing_area_with_pattern.m index eb13e90..c5c190f 100644 --- a/Kod/bilbana/yc4/display/define_flashing_area_with_pattern.m +++ b/Kod/bilbana/yc4/display/define_flashing_area_with_pattern.m @@ -1,5 +1,4 @@ function pkg = define_flashing_area_with_pattern(x1, y1, x2, y2, n1) - arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2), n1]; - - pkg = get_package('QM', arg); +arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2), n1]; +pkg = get_package('QM', arg); end diff --git a/Kod/bilbana/yc4/display/define_touch_key.m b/Kod/bilbana/yc4/display/define_touch_key.m index 14ee5ee..69ed130 100644 --- a/Kod/bilbana/yc4/display/define_touch_key.m +++ b/Kod/bilbana/yc4/display/define_touch_key.m @@ -1,7 +1,7 @@ function pkg = define_touch_key(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; +arg_location = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; +arg_text = double(strjoin({just; text}, '')); +arg_null = 0; - pkg = get_package('AT', [arg_location, down_code, up_code, arg_text, arg_null]); +pkg = get_package('AT', [arg_location, down_code, up_code, arg_text, arg_null]); end diff --git a/Kod/bilbana/yc4/display/define_touch_switch.m b/Kod/bilbana/yc4/display/define_touch_switch.m index 47d3cd9..e45f512 100644 --- a/Kod/bilbana/yc4/display/define_touch_switch.m +++ b/Kod/bilbana/yc4/display/define_touch_switch.m @@ -1,7 +1,7 @@ -function pkg = define_touch_key(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; +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; - pkg = get_package('AK', [arg_location, down_code, up_code, arg_text, arg_null]); +pkg = get_package('AK', [arg_location, down_code, up_code, arg_text, arg_null]); end diff --git a/Kod/bilbana/yc4/display/delete_area.m b/Kod/bilbana/yc4/display/delete_area.m index 44953b7..576638e 100644 --- a/Kod/bilbana/yc4/display/delete_area.m +++ b/Kod/bilbana/yc4/display/delete_area.m @@ -1,5 +1,4 @@ function pkg = delete_area(x1, y1, x2, y2) - arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; - - pkg = get_package('RL', arg); +arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; +pkg = get_package('RL', arg); end diff --git a/Kod/bilbana/yc4/display/draw_box.m b/Kod/bilbana/yc4/display/draw_box.m index b9eb5be..7ac4b70 100644 --- a/Kod/bilbana/yc4/display/draw_box.m +++ b/Kod/bilbana/yc4/display/draw_box.m @@ -1,5 +1,4 @@ function pkg = draw_box(x1, y1, x2, y2, n1) - arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2), n1]; - - pkg = get_package('RO', arg); +arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2), n1]; +pkg = get_package('RO', arg); end diff --git a/Kod/bilbana/yc4/display/draw_frame.m b/Kod/bilbana/yc4/display/draw_frame.m index 7b97adb..1a6ce3b 100644 --- a/Kod/bilbana/yc4/display/draw_frame.m +++ b/Kod/bilbana/yc4/display/draw_frame.m @@ -1,5 +1,4 @@ function pkg = draw_frame(x1, y1, x2, y2, n1) - arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; - - pkg = get_package('RR', arg); +arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; +pkg = get_package('RR', arg); end diff --git a/Kod/bilbana/yc4/display/draw_frame_box.m b/Kod/bilbana/yc4/display/draw_frame_box.m index 9b57c7c..f2a247e 100644 --- a/Kod/bilbana/yc4/display/draw_frame_box.m +++ b/Kod/bilbana/yc4/display/draw_frame_box.m @@ -1,5 +1,4 @@ function pkg = draw_frame_box(x1, y1, x2, y2, n1) - arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2), n1]; - - pkg = get_package('RT', arg); +arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2), n1]; +pkg = get_package('RT', arg); end diff --git a/Kod/bilbana/yc4/display/draw_point.m b/Kod/bilbana/yc4/display/draw_point.m index b81d8cb..34ba683 100644 --- a/Kod/bilbana/yc4/display/draw_point.m +++ b/Kod/bilbana/yc4/display/draw_point.m @@ -1,5 +1,4 @@ function pkg = draw_point(x1, y1) - arg = [get_bytes(x1), get_bytes(y2)]; - - pkg = get_package('GP', arg); +arg = [get_bytes(x1), get_bytes(y1)]; +pkg = get_package('GP', arg); end diff --git a/Kod/bilbana/yc4/display/draw_rectangle.m b/Kod/bilbana/yc4/display/draw_rectangle.m index 9e92cad..3edff7c 100644 --- a/Kod/bilbana/yc4/display/draw_rectangle.m +++ b/Kod/bilbana/yc4/display/draw_rectangle.m @@ -1,5 +1,4 @@ function pkg = draw_rectangle(x1, y1, x2, y2) - arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; - - pkg = get_package('GR', arg); +arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; +pkg = get_package('GR', arg); end diff --git a/Kod/bilbana/yc4/display/draw_single_line.m b/Kod/bilbana/yc4/display/draw_single_line.m index ca13293..c1da8ab 100644 --- a/Kod/bilbana/yc4/display/draw_single_line.m +++ b/Kod/bilbana/yc4/display/draw_single_line.m @@ -1,4 +1,4 @@ function pkg = draw_single_line(x1, y1, x2, y2) - arg = [mod(x1, 255), fix(x1 / 255), y1, 0, mod(x2, 255), fix(x2 / 255), y2, 0]; - pkg = get_package('GD', arg); +arg = [mod(x1, 255), fix(x1 / 255), y1, 0, mod(x2, 255), fix(x2 / 255), y2, 0]; +pkg = get_package('GD', arg); end diff --git a/Kod/bilbana/yc4/display/fill_area.m b/Kod/bilbana/yc4/display/fill_area.m index 4d64463..ae53551 100644 --- a/Kod/bilbana/yc4/display/fill_area.m +++ b/Kod/bilbana/yc4/display/fill_area.m @@ -1,5 +1,4 @@ function pkg = fill_area(x1, y1, x2, y2) - arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; - - pkg = get_package('RS', arg); +arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; +pkg = get_package('RS', arg); end diff --git a/Kod/bilbana/yc4/display/fill_area_with_pattern.m b/Kod/bilbana/yc4/display/fill_area_with_pattern.m index 4815f5b..a461444 100644 --- a/Kod/bilbana/yc4/display/fill_area_with_pattern.m +++ b/Kod/bilbana/yc4/display/fill_area_with_pattern.m @@ -1,5 +1,4 @@ function pkg = fill_area_with_pattern(x1, y1, x2, y2, n1) - arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2), n1]; - - pkg = get_package('RM', arg); +arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2), n1]; +pkg = get_package('RM', arg); end diff --git a/Kod/bilbana/yc4/display/fill_display.m b/Kod/bilbana/yc4/display/fill_display.m index e5fc1d2..55699b2 100644 --- a/Kod/bilbana/yc4/display/fill_display.m +++ b/Kod/bilbana/yc4/display/fill_display.m @@ -1,3 +1,3 @@ function pkg = fill_display() - pkg = get_package('DS', []); +pkg = get_package('DS', []); end diff --git a/Kod/bilbana/yc4/display/get_bytes.m b/Kod/bilbana/yc4/display/get_bytes.m index 76efc9c..9482008 100644 --- a/Kod/bilbana/yc4/display/get_bytes.m +++ b/Kod/bilbana/yc4/display/get_bytes.m @@ -1,3 +1,3 @@ -function bytes = get_bytes(num): - bytes = [mod(num, 256), num ./ 256]; +function bytes = get_bytes(num) +bytes = [mod(num, 256), fix(num / 256)]; end diff --git a/Kod/bilbana/yc4/display/get_num.m b/Kod/bilbana/yc4/display/get_num.m new file mode 100644 index 0000000..8ff62f7 --- /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 diff --git a/Kod/bilbana/yc4/display/get_package.m b/Kod/bilbana/yc4/display/get_package.m index 7c3a01e..d2e8a26 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; - ESC = 27; - - data = [ESC, double(code), args]; - len = length(data); - initStr = [DC1, len, data]; - bcc = mod(sum(initStr), 256); - pkg = [initStr, bcc]; +function pkg = get_package(code, args) +ESC = 27; +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..0c77a00 --- /dev/null +++ b/Kod/bilbana/yc4/display/get_response.m @@ -0,0 +1,56 @@ +function [ack, start_code, responses] = get_response(display_data) +% GET RESPONSE +% In-depth explanation +% [flag, display_data] = matlabclient(2) + +ack = false; +start_code = ''; +responses = []; + +if isempty(display_data) + return +end +bcc = display_data(length(display_data)); + + +pointer = 1; + +if display_data(pointer) == 6 + ack = true; +else + return +end + +if pointer > length(display_data) + return +end + +pointer = pointer + 1; +if display_data(1) == 17 + start_code = 'DC1'; +elseif display_data(1) == 18 + start_code = 'DC2'; +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/display/get_smallpackage.m b/Kod/bilbana/yc4/display/get_smallpackage.m new file mode 100644 index 0000000..be386bc --- /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/invert_area.m b/Kod/bilbana/yc4/display/invert_area.m index 39db3c2..081d65c 100644 --- a/Kod/bilbana/yc4/display/invert_area.m +++ b/Kod/bilbana/yc4/display/invert_area.m @@ -1,5 +1,4 @@ function pkg = invert_area(x1, y1, x2, y2) - arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; - - pkg = get_package('RI', arg); +arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; +pkg = get_package('RI', arg); end diff --git a/Kod/bilbana/yc4/display/invert_display.m b/Kod/bilbana/yc4/display/invert_display.m index ab2ddad..c661167 100644 --- a/Kod/bilbana/yc4/display/invert_display.m +++ b/Kod/bilbana/yc4/display/invert_display.m @@ -1,3 +1,3 @@ function pkg = invert_display() - pkg = get_package('DI', []); +pkg = get_package('DI', []); end diff --git a/Kod/bilbana/yc4/display/put_text.m b/Kod/bilbana/yc4/display/put_text.m index 22871e3..8455c83 100644 --- a/Kod/bilbana/yc4/display/put_text.m +++ b/Kod/bilbana/yc4/display/put_text.m @@ -1,9 +1,7 @@ function [pkg] = put_text(x, y, justification, text) - code = strjoin({'Z', justification}, ''); +arg1 = [mod(x, 256), x ./ 256, mod(y, 256), y ./ 256]; +arg2 = text; +arg3 = 0; - arg1 = [mod(x, 256), x ./ 256, mod(y, 256), y ./ 256]; - arg2 = text; - arg3 = 0; - - pkg = get_package(code, [arg1 double(arg2) arg3]); +pkg = get_package(strjoin({'Z', justification}, ''), [arg1 double(arg2) arg3]); end diff --git a/Kod/bilbana/yc4/display/remove_flashing_area.m b/Kod/bilbana/yc4/display/remove_flashing_area.m index 701402b..4efa5be 100644 --- a/Kod/bilbana/yc4/display/remove_flashing_area.m +++ b/Kod/bilbana/yc4/display/remove_flashing_area.m @@ -1,5 +1,4 @@ function pkg = remove_flashing_area(x1, y1, x2, y2) - arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; - - pkg = get_package('QL', arg); +arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; +pkg = get_package('QL', arg); end diff --git a/Kod/bilbana/yc4/display/restore_display_from_clipboard.m b/Kod/bilbana/yc4/display/restore_display_from_clipboard.m index 9542851..44c6150 100644 --- a/Kod/bilbana/yc4/display/restore_display_from_clipboard.m +++ b/Kod/bilbana/yc4/display/restore_display_from_clipboard.m @@ -1,3 +1,3 @@ function pkg = restore_display_from_clipboard() - pkg = get_package('CR', []); +pkg = get_package('CR', []); end diff --git a/Kod/bilbana/yc4/display/restore_display_from_clipboard_to_point.m b/Kod/bilbana/yc4/display/restore_display_from_clipboard_to_point.m index 2c9e1aa..e10d019 100644 --- a/Kod/bilbana/yc4/display/restore_display_from_clipboard_to_point.m +++ b/Kod/bilbana/yc4/display/restore_display_from_clipboard_to_point.m @@ -1,5 +1,4 @@ function pkg = restore_display_from_clipboard_to_point(x1, y1) - arg = [get_bytes(x1), get_bytes(y1)]; - - pkg = get_package('CK', arg); +arg = [get_bytes(x1), get_bytes(y1)]; +pkg = get_package('CK', arg); end diff --git a/Kod/bilbana/yc4/display/save_area_to_clipboard.m b/Kod/bilbana/yc4/display/save_area_to_clipboard.m index 8098b06..4d12459 100644 --- a/Kod/bilbana/yc4/display/save_area_to_clipboard.m +++ b/Kod/bilbana/yc4/display/save_area_to_clipboard.m @@ -1,5 +1,4 @@ function pkg = save_area_to_clipboard(x1, y1, x2, y2) - arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; - - pkg = get_package('CS', arg); +arg = [get_bytes(x1), get_bytes(y1), get_bytes(x2), get_bytes(y2)]; +pkg = get_package('CS', arg); end diff --git a/Kod/bilbana/yc4/display/save_display_to_clipboard.m b/Kod/bilbana/yc4/display/save_display_to_clipboard.m index f8cd0bd..404831f 100644 --- a/Kod/bilbana/yc4/display/save_display_to_clipboard.m +++ b/Kod/bilbana/yc4/display/save_display_to_clipboard.m @@ -1,3 +1,3 @@ function pkg = save_display_to_clipboard() - pkg = get_package('CB', []); +pkg = get_package('CB', []); end diff --git a/Kod/bilbana/yc4/display/send_data_to_display.m b/Kod/bilbana/yc4/display/send_data_to_display.m deleted file mode 100644 index c5d70c7..0000000 --- a/Kod/bilbana/yc4/display/send_data_to_display.m +++ /dev/null @@ -1,27 +0,0 @@ -function [] = send_data_to_display() -%SEND_DATA_TO_DISPLAY sends available data to display if last send was -% more than 0.5 seconds ago. - persistent last_send; - global display_data; - - if isempty(display_data) - return - end - % disp(last_send); - % disp(clock); - if isempty(last_send) % first send - %% SEND DATA - % disp('sending data'); - % disp(display_data) - matlabclient(1, display_data{1}); - last_send = clock; - display_data(1) = []; - elseif (etime(clock, last_send) >= 0.5) - %% SEND DATA - % disp('sending data'); - % disp(display_data) - matlabclient(1, display_data{1}); - last_send = clock; - display_data(1) = []; - end -end diff --git a/Kod/bilbana/yc4/display/set_display_visible.m b/Kod/bilbana/yc4/display/set_display_visible.m index 12fa3b3..58cd1a4 100644 --- a/Kod/bilbana/yc4/display/set_display_visible.m +++ b/Kod/bilbana/yc4/display/set_display_visible.m @@ -1,7 +1,7 @@ function pkg = set_display_visible(visible) - if visible == true - pkg = get_package('DE', []); - else - pkg = get_package('DA', []); - end +if visible == true + pkg = get_package('DE', []); +else + pkg = get_package('DA', []); +end end diff --git a/Kod/bilbana/yc4/display/set_drawing_mode.m b/Kod/bilbana/yc4/display/set_drawing_mode.m index c8ecf07..1164ebc 100644 --- a/Kod/bilbana/yc4/display/set_drawing_mode.m +++ b/Kod/bilbana/yc4/display/set_drawing_mode.m @@ -1,5 +1,6 @@ function pkg = set_drawing_mode(n1) - arg = [n1]; - - pkg = get_package('GV', arg); +% 1 = set +% 2 = delete (erase) +% 3 = invert (on -> off, off -> on for every pixel thats drawn over) +pkg = get_package('GV', [n1]); end diff --git a/Kod/bilbana/yc4/display/set_flashing_time.m b/Kod/bilbana/yc4/display/set_flashing_time.m index e4f056d..3d11420 100644 --- a/Kod/bilbana/yc4/display/set_flashing_time.m +++ b/Kod/bilbana/yc4/display/set_flashing_time.m @@ -1,3 +1,3 @@ function pkg = set_flashing_time(n1) - pkg = get_package('QZ', [n1]); +pkg = get_package('QZ', [n1]); end diff --git a/Kod/bilbana/yc4/display/set_line_pattern.m b/Kod/bilbana/yc4/display/set_line_pattern.m new file mode 100644 index 0000000..c836c80 --- /dev/null +++ b/Kod/bilbana/yc4/display/set_line_pattern.m @@ -0,0 +1,3 @@ +function pkg = set_line_pattern(n1) +pkg = get_package('GM', n1); +end diff --git a/Kod/bilbana/yc4/display/set_pattern.m b/Kod/bilbana/yc4/display/set_pattern.m index 5ec583e..5962ebc 100644 --- a/Kod/bilbana/yc4/display/set_pattern.m +++ b/Kod/bilbana/yc4/display/set_pattern.m @@ -1,5 +1,3 @@ function pkg = set_pattern(n1) - arg = [n1]; - - pkg = get_package('GM', n1); +pkg = get_package('GM', n1); end diff --git a/Kod/bilbana/yc4/display/set_point_size.m b/Kod/bilbana/yc4/display/set_point_size.m index 750b172..17615a1 100644 --- a/Kod/bilbana/yc4/display/set_point_size.m +++ b/Kod/bilbana/yc4/display/set_point_size.m @@ -1,5 +1,4 @@ function pkg = set_point_size(n1, n2) - arg = [n1 n2]; - - pkg = get_package('GZ', arg); +arg = [n1, n2]; +pkg = get_package('GZ', arg); end diff --git a/Kod/bilbana/yc4/display/set_text_flashing.m b/Kod/bilbana/yc4/display/set_text_flashing.m new file mode 100644 index 0000000..00f683e --- /dev/null +++ b/Kod/bilbana/yc4/display/set_text_flashing.m @@ -0,0 +1,7 @@ +function pkg = set_text_flashing(n1) +%SET_TEXT_FLASHING Summary of this function goes here +% Detailed explanation goes here + +pkg = get_package('ZB', [n1]); +end + diff --git a/Kod/bilbana/yc4/display/set_text_font.m b/Kod/bilbana/yc4/display/set_text_font.m index 768d16b..1aed263 100644 --- a/Kod/bilbana/yc4/display/set_text_font.m +++ b/Kod/bilbana/yc4/display/set_text_font.m @@ -1,3 +1,3 @@ function pkg = set_text_font(font_num) - pkg = get_package('ZF', [font_num]) +pkg = get_package('ZF', [font_num]); end diff --git a/Kod/bilbana/yc4/display/set_touch_sound_response.m b/Kod/bilbana/yc4/display/set_touch_sound_response.m index 851eaf2..cb14d83 100644 --- a/Kod/bilbana/yc4/display/set_touch_sound_response.m +++ b/Kod/bilbana/yc4/display/set_touch_sound_response.m @@ -1,7 +1,7 @@ function pkg = set_touch_sound_response(state) - if state == true - pkg = get_package('AS', [1]); - else - pkg = get_package('AS', [0]); - end +if state == true + pkg = get_package('AS', [1]); +else + pkg = get_package('AS', [0]); +end end diff --git a/Kod/bilbana/yc4/do_car.m b/Kod/bilbana/yc4/do_car.m new file mode 100644 index 0000000..f822395 --- /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 new file mode 100644 index 0000000..1867565 --- /dev/null +++ b/Kod/bilbana/yc4/get_aprox_v.m @@ -0,0 +1,32 @@ +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 +%} +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 = []; + +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 26bf175..e722326 100644 --- a/Kod/bilbana/yc4/get_new_v.m +++ b/Kod/bilbana/yc4/get_new_v.m @@ -1,21 +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. %{ -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. %} +position = position*100; %temp input för position -%{ -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); -end +for i = 1:length(list) + + if list(i,1) > position + new_v = list((i-1),4); + break + elseif i == length(list) + new_v = list(80,4); + break + 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 3bdf697..109d8b4 100644 --- a/Kod/bilbana/yc4/main.m +++ b/Kod/bilbana/yc4/main.m @@ -5,39 +5,94 @@ 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; +display.send_interval = 0.5; 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) car1 = struct; +car1.num = 1; +car1.running = false; +car1.automatic = true; 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; car2 = struct; +car2.num = 2; +car2.running = false; +car2.automatic = true; 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; + +t = 0; +highToc = 0; + +%% ASK ACTIVE CARS +disp('J = Ja (automatiskt), M = Ja (manuellt), N = Nej'); + +car1.response = input('Vill du köra bil 1? [N] ', 's'); +if car1.response == 'J' + car1.running = true; + car1.automatic = true; +elseif car1.response == 'M' + car1.running = true; + car1.automatic = false; +else + car1.running = false; +end + + +car2.response = input('Vill du köra bil 2? [N] ', 's'); +if car2.response == 'J' + car2.running = true; + car2.automatic = true; +elseif car2.response == 'M' + car2.running = true; + car2.automatic = false; +else + car2.running = false; +end -tocs = []; %% MAIN LOOP while 1 - tic; + readTime = tic; %% PRE-LOOP if strcmp(get(hf,'currentcharacter'),'q') close(hf) @@ -47,102 +102,64 @@ while 1 figure(hf) drawnow - %% READ - [car1.new_lap, car1.new_check_point, car1.time] = get_car_position(1); - [car2.new_lap, car2.new_check_point, car2.time] = get_car_position(2); - - %% CHECK LAP AND CHECKPOINT (CAR 1) - 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; + [car1, car1.stop, display.data] = do_car(car1, t, display.data); + [car2, car2.stop, display.data] = do_car(car2, t, display.data); - 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; - - car1.seg_times(car1.lap, 1) = 0; - end + if car1.stop == true + disp('stopped by car 1'); + break; + end + if car2.stop == true + disp('stopped by car 2'); + break; + end - %% CHECK LAP AND CHECKPOINT (CAR 2) - 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 OF LOOP + while 1 %Whileloop med paus som k�rs till pausen �verskridit 0.07 sekunder + % DISPLAY + display.send_delay = tic; + if toc(display.last_send) > display.send_interval + % queue control signal + if car1.running && car1.automatic + display.data = [display.data, put_text(20, 16 + (16 * 1), 'L', num2str(car1.u))]; + end + if car2.running && car2.automatic + display.data = [display.data, put_text(20, 16 + (16 * 2), 'L', num2str(car2.u))]; + end + + % 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 - 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))}, ' '))}; + % disp(strjoin({'display took additional ', num2str(toc(display.send_delay))})); + % ACTUAL END OF LOOP + t = toc(readTime); - car2.segment = 1; - car2.lap = car2.lap + 1; - - car2.seg_times(car2.lap, 1) = 0; + 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 + pause(0.001); end - - %% CALCULATE (CAR 1) - car1.car_constant = get_car_constant(1); - car1.v = get_new_v(car1.segment); - car1.track_u_constant = get_track_u_constant(); - car1.u = get_new_u(car1.v, car1.car_constant, car1.track_u_constant); - - %% CALCULATE (CAR 2) - car2.car_constant = get_car_constant(2); - car2.v = get_new_v(car2.segment); - car2.track_u_constant = get_track_u_constant(); - car2.u = get_new_u(car2.v, car2.car_constant, car2.track_u_constant); - - %% EXECUTE - set_car_speed(1, car1.u); - set_car_speed(2, car2.u); - - %% DISPLAY - - %% END OF LOOP - pause(0.1) - tocs(end + 1) = toc; - send_data_to_display(); end %% END OF PROGRAM -disp(tocs); +disp(highToc); disp(car1); disp(car2); @@ -153,5 +170,11 @@ matlabclient(3); %% DISPLAY GRAPHS -graphs(car1.lap_times, 13, car1.seg_times, 1); -graphs(car2.lap_times, 13, car2.seg_times, 2);
\ No newline at end of file +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 |