gusucode.com > Mobile Robotics Simulation Toolbox > examples/matlab/multirobot/mrsSwarmTeams.m
%% EXAMPLE: Multi-Robot Swarm Behavior % Copyright 2018 The MathWorks, Inc. %% Create a multi-robot environment numRobots = 50; env = MultiRobotEnv(numRobots); env.robotRadius = 0.15; env.showTrajectory = false; %% Create robot detectors for all robots detectors = cell(1,numRobots); for rIdx = 1:numRobots detector = RobotDetector(env,rIdx); detector.maxDetections = numRobots; detector.maxRange = 10; detector.fieldOfView = pi/2; detectors{rIdx} = detector; end env.plotSensorLines = false; % So the sensor lines don't dominate the visuals %% Initialization % Number of robot teams numTeams = 5; env.robotColors = repmat(hsv(numTeams),[ceil(numRobots/numTeams) 1]); env.robotColors = env.robotColors(1:numRobots,:); % Truncate colors in case there are unequal teams sampleTime = 0.1; % Sample time [s] tVec = 0:sampleTime:25; % Time array % Initialize poses randomly, and add bias to each "team" poses = [10*(rand(2,numRobots) - 0.5); ... pi*rand(1,numRobots)]; angleBias = 2*pi*(1:numRobots)/numTeams; poses(1:2,:) = poses(1:2,:) + 2.5*[sin(angleBias);cos(angleBias)]; %% Simulation loop vel = zeros(3,numRobots); for idx = 2:numel(tVec) % Update the environment env(1:numRobots, poses); xlim([-8 8]); % Without this, axis resizing can slow things down ylim([-8 8]); % Read the sensor and execute the controller for each robot for rIdx = 1:numRobots detections = step(detectors{rIdx}); vel(:,rIdx) = swarmTeamController(poses,rIdx,detections,numTeams); end % Discrete integration of pose poses = poses + vel*sampleTime; end %% Helper function: Robot Controller Logic function vel = swarmTeamController(poses,rIdx,detections,numTeams) % Unpack the robot's pose and team index pose = poses(:,rIdx); teamIdx = mod(rIdx,numTeams); % If there are no detections, turn in place v = 0; w = 2; % Else, turn towards the average of detected robots on the same team if ~isempty(detections) validInds = find(mod(detections(:,3),numTeams) == teamIdx); if ~isempty(validInds) % Take the average range and angle range = mean(detections(validInds,1)); angle = mean(detections(validInds,2)); % Move linearly to maintain a range to the nearest robot if range > 0.6 v = 0.5; elseif range < 0.4 v = -0.5; end % Turn to maintain a heading to the nearest robot if angle > pi/12 w = 2; elseif angle < -pi/12 w = -2; end end end % Convert to global velocity vel = bodyToWorld([v;0;w],pose); end