mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-15 15:28:32 +00:00
33945f6414
- Deliverables folder holds final files for submission - Experimentation holds scripts and things related to our development of a final solution - Simulation holds the provided files - Added template for part 2 submission with comments
27 lines
856 B
Matlab
27 lines
856 B
Matlab
function Xobs_seen = senseObstacles(curr_pos, Xobs)
|
|
% Xobs_seen = senseObstacles(curr_pos, Xobs)
|
|
%
|
|
% Given the current vehicle position, sense the obstacles within 150m.
|
|
%
|
|
% INPUTS:
|
|
% curr_pos a 2-by-1 vector where the 1st and 2nd elements represent the
|
|
% x and y coordinates of the current vehicle position
|
|
%
|
|
% Xobs a cell array generated by generateRandomObstacles.m
|
|
%
|
|
% OUTPUTS:
|
|
% Xobs_seen a cell array which contains all obstacles that are no
|
|
% greater than 150m from the vehicle. Each cell has the same
|
|
% structure as the cells in Xobs.
|
|
%
|
|
% Written by: Jinsun Liu
|
|
% Created: 31 Oct 2021
|
|
|
|
|
|
|
|
|
|
Xobs_mat = cell2mat(Xobs');
|
|
dist = (Xobs_mat(:,1) - curr_pos(1)).^2 + (Xobs_mat(:,2) - curr_pos(2)).^2;
|
|
idx = unique(ceil(find(dist<=150^2)/4));
|
|
Xobs_seen = {Xobs{idx}};
|
|
end |