How do I solve an algebraic loop error in Simulink?

Question

0 个投票

In brief, I am trying to create a simulink function block that selects the next waypoint in a predefined array when the model vehicle gets within a certain distance of the current waypoint.

Inputs into this function block are the current waypoint 'target' in the form of a [1 x 2] array, distance to the currently selected waypoint 'd2w' and 'accuracy' as a value at which when 'd2w' is less than 'accuracy', the function block switches to the next waypoint in the predefined array.

Output is simply the desired waypoint 'targetnew'.

function targetnew = setwaypoint(d2w,target,accuracy)
    waypoints = [100 150; 200 400; 175 275; 600 300]; % pre-defined waypoints (in the future imported as a large array from an external source).
    
    if isempty(target)
        target = waypoints(1,:); % initializes the first waypoint.
    end
    
    Lia = ismember(waypoints,target,'rows'); % identifies if each row of waypoints is equal to target.
    idxa = find(Lia); % indexes waypoints rows that are equal to target.
    
    if isempty(idxa)
        idx = 1; % prevents error when idxa is empty.
    else 
        idx = idxa(1); % selects only the first matched row in case more exist.
    end 
    
    if d2w > accuracy
        idx = idx + 1; % if target waypoint is reached, add one to index.
    end
    
    if idx > size(waypoints, 1) % attempt to avoid any issues if simulation time is greater than time taken to reach final waypoint.
    targetnew = waypoints(end, :);
    else
    targetnew = waypoints(idx, :);
    end 
    
end