www.gusucode.com > Arduino_Engineering_Kit_Project_Files工具箱matlab程序 > Arduino_Engineering_Kit_Project_Files/MobileRover/Exercise5/roverPosAng.m
function [x,y,heading] = roverPosAng(IMG,P) % Copyright 2018 The MathWorks, Inc. % Create a disk object (will be used to clean up the image) and determine the RGB threshold values for the image delta = 20; s = strel('disk',3); threshR = P(1,:) + delta.*[-1 1 1]; threshG = P(2,:) + delta.*[1 -1 1]; threshB = P(3,:) + delta.*[1 1 -1]; % Threshold each channel of the image Rin = IMG(:,:,1) > threshR(1) & IMG(:,:,2) < threshR(2) & IMG(:,:,3) < threshR(3); Gin = IMG(:,:,1) < threshG(1) & IMG(:,:,2) > threshG(2) & IMG(:,:,3) < threshG(3); Bin = IMG(:,:,1) < threshB(1) & IMG(:,:,2) < threshB(2) & IMG(:,:,3) > threshB(3); % Remove RGB points that do not have a radius of 3 pixels Rf = imopen(Rin,s); R = imclose(Rf,s); Gf = imopen(Gin,s); G = imclose(Gf,s); Bf = imopen(Bin,s); B = imclose(Bf,s); %% Position Calculation (Getting the centroid of the circular plate) % Get the red centroid sR = regionprops(R,'centroid'); cR = sR.Centroid; % Get the green centroid sG = regionprops(G,'centroid'); cG = sG.Centroid; % Get the blue centroid sB = regionprops(B,'centroid'); cB = sB.Centroid; % Determine where the center of the rover's colored wheel is located cPlate = (cR + cG + cB)/3; %% Heading calculation (Getting the orientation of the rover) % Assumption: The origin of the arena is in the bottom left corner. % The markers are placed in equilateral triangle. % The red marker should correspond to the front of the rover % The blue marker should correspond to the base of the rover thetaRC = atand((cPlate(2) - cR(2)) / (cPlate(1) - cR(1))); thetaFinal = thetaRC; % Add 180 offset if red marker lies to the left of centre, given the % conventions noted earlier. if cR(1) < cPlate(1) thetaFinal = thetaFinal + 180 ; end thetaFinal = thetaFinal + 180; % Output the heading and (x,y) coordinates of the rover x = cPlate(1); y = cPlate(2); heading = thetaFinal; if heading > 360 heading = heading - 360; end