From 6dd1c252429762488fb437c36ae98292f8da64d3 Mon Sep 17 00:00:00 2001 From: Albin Date: Wed, 6 Nov 2019 19:49:39 +0100 Subject: choose position --- Kod/bilbana/yc4/choose_position.m | 39 +++++++++++++++++++++++++++++++++------ 1 file changed, 33 insertions(+), 6 deletions(-) diff --git a/Kod/bilbana/yc4/choose_position.m b/Kod/bilbana/yc4/choose_position.m index 36e4657..1c44853 100644 --- a/Kod/bilbana/yc4/choose_position.m +++ b/Kod/bilbana/yc4/choose_position.m @@ -1,9 +1,36 @@ -function [outputArg1,outputArg2] = choose_position(position,segment,t_missed) +function [new_position, seg_plus] = choose_position(position,segment,t_missed, track) %CHOOSE_POSITION Välj vad position ska vara -% Väljer position om ingen indata. Om indata kollar den om indatan är -% rimlig eller om någon givare missats. Sedan väljs position efter vilken -% givare det var som passerades. -outputArg1 = inputArg1; -outputArg2 = inputArg2; +% 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 = [19.60 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]; +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]; +pos_c = position; +pos_i = track_len( track, segment); +%% 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(i) + near = [i,diff]; + end + end +end +%% Beräkning av passerad givare +if near(1) == segment + new_position = set_pos( track, segment); + seg_plus = 0; +else + seg_plus = near(1) - segment + new_position = set_pos( track, segment) +end +% TODO Bättre att utgå ifrån ingen missad givare ifall ingen annan givare +% är särskillt nära pos_c heller. (Även om det är närmare) end -- cgit v1.2.1