MatLabscript uitbaldetectie

From Control Systems Technology Group
Revision as of 21:00, 14 January 2016 by S136448 (talk | contribs)
Jump to navigation Jump to search

close all; clear camObj; 

w=1;
d=0;
v=1
; %% Setup
clear camObj
if w camObj=gigecam; end
coeffO = 5;
coeffC = 5;
centr_drone = [];
angle=0;
videoPlayer=vision.VideoPlayer('Name','Video','Position', [10, 10, 720, 480]);
foregrounddetect = vision.ForegroundDetector('NumGaussians', 3, ...
'NumTrainingFrames', 10, 'MinimumBackgroundRatio', 0.7);
nr=0;

%setup blob analyzer
blob_ball = vision.BlobAnalysis('BoundingBoxOutputPort', true, ...
'MajorAxisLengthOutputPort',true,'EccentricityOutputPort', true, 'CentroidOutputPort', true, ...
'MinimumBlobArea', 100, 'MaximumBlobArea',1000);
blob_drone = vision.BlobAnalysis('BoundingBoxOutputPort', true, ...
'CentroidOutputPort', true, 'MajorAxisLengthOutputPort',true,'MinorAxisLengthOutputPort',true,'OrientationOutputPort',true, ...
'MinimumBlobArea', 1000, 'MaximumBlobArea',4000);
if ~w videoReader = vision.VideoFileReader('topcam_drone.mp4'); end

%Coordinates of field corners and goal points bottom_left=[168, 1049]; %[x, y] bottom_right=[1699, 1008]; %[x, y] upper_left=[155, 5]; %[x,y] upper_right=[1685, 5]; %[x,y] gl_up=370; %[y] gl_down=670; %[y] gr_up=350; %[y] gr_down=630; %[y] %Creating a formula for y, dependent on x xdiff_down=bottom_right(1)-bottom_left(1); ydiff_up=bottom_left(2)-bottom_right(2); xdiff_left=bottom_left(1)-upper_left(1); ydiff_left=bottom_left(2)-upper_left(2); xdiff_right=upper_right(1)-bottom_right(1); ydiff_right=upper_right(2)-bottom_right(2); %Loading in sound file my_sound = audioread('whistle.wav'); cheering=audioread('cheering.wav'); my_player = audioplayer(my_sound, 44100); cheering = audioplayer(cheering, 44100)
%% Start Analysis

if ~v
img=snapshot(camObj);
step(videoPlayer,img);
end
centr_prev = [0;0];
angle_prev = 0;

if d
drone.takeoff(); end

while (isOpen(videoPlayer)) || nr==0 nr=nr+1; if nr==5 release(foregrounddetect); elseif nr>5 && nr<8 && d moveForward(drone,0.5); end %Read one frame if w img=snapshot(camObj); else img=rgb2gray(step(videoReader)); end %Filter out parts outside of the field img = img(:,1:end-100); img(img>200)=90; foreground = foregrounddetect.step(img); %foreground = imopen(foreground, strel('rectangle', [coeffO,coeffO])); foreground = imclose(foreground, strel('rectangle', [coeffC, coeffC])); if ~isempty(centr_drone) centr_prev=centr_drone; ball_prev = centr_ball; angle_prev = angle; else centr_prev = [0;0]; angle_prev = angle; ball_prev = [0;0]; end %3a: detect blobs, return centroids, bounding boxes, eccentricity and %diameter [~,centr_ball,bbox_ball,diam,ecc] = step(blob_ball,foreground); %For ball [~,centr_drone,bbox_drone,Ma,Mi,angle] = step(blob_drone,foreground); %For drone %3b: maximize for most round object for ball if ~isempty(centr_ball) [ecmin,I] = min(ecc,[],1); centr_ball = centr_ball(I,:); diam = diam(I); else centr_ball = [0 0]; diam = 0; ecmin=1e10; end if ecmin>0.7 centr_ball = [0 0]; diam = 0; %disp('Noise!'); else %disp('Ball detected'); end
  %% Line Detection
  down_line=(bottom_left(2)+(ydiff_up/xdiff_down)*bottom_left(1))-(ydiff_up/xdiff_down)*centr_ball(1);
  left_line=(upper_left(1)+(xdiff_left/ydiff_left)*upper_left(2))+(xdiff_left/ydiff_left)*centr_ball(2);
  right_line=(bottom_right(1)+(xdiff_right/ydiff_right)*bottom_right(2))+(xdiff_right/ydiff_right)*centr_ball(2);
  
  %IF loop (if the ball is above the top line, play the whistle)
  if ball_prev(1)>0 && ball_prev(2)>0 && ball_prev(2)<200 && centr_ball(1)==0 && centr_ball(2)==0
      if ~isplaying(my_player)
          play(my_player);
          disp('Out! Top ');
      end       
  end
  
  if centr_ball(1)~=0 && centr_ball(2)~=0
  
  %IF loop (if the ball is above the bottom line, play the whistle)       
  if centr_ball(2)>=down_line
      if ~isplaying(my_player)
          play(my_player);
          disp('Out! Down');
      end     
  end %out of field down line
  
  %IF loop (if the ball is in the left goal, play the GOLGOLGOL-tune)   
  if (centr_ball(1)<=left_line && centr_ball(2)>=gl_up && centr_ball(1)<=left_line && centr_ball(2)<=gl_down)
      if ~isplaying(cheering)
          play(cheering);
          disp('GOAL! - Left');
      end       
  elseif centr_ball(1)==0
      [];
  end %goal left half
  
  %IF loop (if the ball is in the right goal, play the GOLGOLGOL-tune)    
  if (centr_ball(1)>=right_line && centr_ball(2)>=gr_up && centr_ball(1)>=right_line && centr_ball(2)<=gr_down)
      if ~isplaying(cheering)
          play(cheering);
          disp('GOAL! - Right');
      end       
  elseif centr_ball(1)==0
      [];
  end %goal right half
  
  %IF loop (if the ball is right of the right line, play the whistle)
  if (centr_ball(1)>=right_line && centr_ball(2)<=gr_up) || (centr_ball(1)>=right_line && centr_ball(2)>=gr_down)
      if ~isplaying(my_player)
          play(my_player);
          disp('Out! Right');
      end       
  elseif centr_ball(1)==0
      [];
  end %out of field right
%IF loop (if the ball is left of the left line, play the whistle)
   if (centr_ball(1)<=left_line && centr_ball(2)<=gl_up) || (centr_ball(1)<=left_line && centr_ball(2)>=gl_down)
      if ~isplaying(my_player)
          play(my_player);
          disp('Out! Left')
      end      
  elseif centr_ball(1)==0
      [];
  end %out of field left
  end
  test = [0 0];
  test(1) = centr_drone(1) + 200*cosd(angle);
  test(2) = centr_drone(2) + 200*sind(angle);
  
  if v
   foreground = insertShape(1.*foreground, 'Rectangle', bbox_drone);
   foreground = insertShape(1.*foreground, 'FilledCircle', [centr_ball diam./2],'SmoothEdges',false,'Color', [255,0,0]);
   %foreground = insertShape(1.*foreground, 'Line', [centr_drone,test]);
   
   %Displaying the yellow lines for clarification
   img = insertShape(1.*img, 'Line',[bottom_left,bottom_right],'LineWidth', 5);
   img = insertShape(1.*img, 'Line',[upper_left,bottom_left],'LineWidth', 5);  
   img = insertShape(1.*img, 'Line',[upper_right,bottom_right],'LineWidth', 5);   
   step(videoPlayer,foreground);
  end
  
end

if d
drone.land(); end
release(videoPlayer);