MatLabscript uitbaldetectie

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
  
  %3b: for drone
  if ~isempty(centr_drone)
        sq = abs(Ma-Mi);
        [~,I]=min(sq);
        bbox_drone = bbox_drone(I,:);
        centr_drone = centr_drone(I,:);
        angle = angle(I)/pi*180;
        
        if (abs(centr_drone(2)-centr_prev(2))<200 && abs(centr_drone(1)-centr_drone(1))<200)
           angle_calc = atan2((centr_drone(2)-centr_prev(2)),(centr_drone(1)-centr_prev(1)))/pi*180;
        end
        
        if (abs(angle-angle_prev)>90) 
            angle=angle_prev; 
        end
        
        %Angle normalisation
        if (centr_ball(1)~=0 && centr_ball(2)~=0)
          if (centr_ball(1)<centr_drone(1) && centr_ball(2)<centr_drone(2))
              angle = angle+180;
          elseif (centr_ball(1)<centr_drone(1) && centr_ball(2)>centr_drone(2))
              angle = angle-180;
          end
        end
        if d
        droneflypath_demo(drone,nr,centr_drone,centr_ball);
        end
  else
       centr_drone = [0 0];
       angle = 0;
       bbox_drone = [0 0 0 0];
  end
  
  %% Line Detection

  %IF loop (if the ball is out of the field, do ...
  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 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 centr_ball(2)>=down_line
      if ~isplaying(my_player)
          play(my_player);
          disp('Out! Down');
      end     
  end %out of field down line
  
  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 (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 (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 (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]);
   
   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);