Matlabscript DroneCamera

From Control Systems Technology Group
Revision as of 00:55, 13 January 2016 by S132439 (talk | contribs)
Jump to navigation Jump to search

function dronecamdata(drone, Xball_l,Yball_l, Diam)

   % Dronecamdata is a function that interprets the output data from the 
   % video analysis and converts it to the drone's movement
.
   if (Xball_l == 0 && Yball_l == 0) 
      % do nothing if ball is undetected
       drone.drive([0,0,0,0]);
   else
   Xref = 640/2;  % middle of the screen
   Yref = 0;
   Diamref_min = 70;  
   Diamref_max = 90;
   x_l_target = Xball_l - Xref;  
   y_l_target = Yball_l - Yref;
.
   %% Calling upon the drone's drive function
.
       rSpeed   = 0; %right/left turn speed
       revSpeed = 0; %reverse/forward speed
.
   % a quadratic correlation is used to reposition the drone.
   % the further the distance from the reference line, the faster it moves
.
       if x_l_target > 0
            rSpeed =  (0.000015*(x_l_target)^2);  
            disp('turning right')
       elseif x_l_target < 0
            rSpeed = - (0.000015*(x_l_target)^2);
            disp('turning left')
       end
.
   % to ensure the right distance, the drone has to get the diameter of the
   % ball to be between a certain range, otherwise act as necessary
   if 1
       if Diam > Diamref_max;
            revSpeed = max(0.0003*(Diam-Diamref_max)^2,0.3);   %backwards
       elseif Diam < Diamref_min
            revSpeed = - max(0.0003*(Diam-Diamref_min)^2,0.3); %forward
       end
   end
.
       drone.drive([revSpeed, 0, 0, rSpeed ]);
   end

end