Matlabscript TopCamera: Difference between revisions

From Control Systems Technology Group
Jump to navigation Jump to search
(Created page with '<code style="font-size:18px"> function droneflypath_demo(drone,nr, centr_drone, centr_ball) %% create direction vector if centr_ball(1) == 0 && centr_ball(2) == 0 …')
 
No edit summary
Line 21: Line 21:
           spBack    = 0;       
           spBack    = 0;       
           max_speed = 0.4;   
           max_speed = 0.4;   
 
.
             if Dv(1)>50
             if Dv(1)>50
                 spRight = max(Dv(1)*0.001*0.5,max_speed); end  %right
                 spRight = max(Dv(1)*0.001*0.5,max_speed); end  %right

Revision as of 00:58, 13 January 2016

function droneflypath_demo(drone,nr, centr_drone, centr_ball)

   %% create direction vector 
   if centr_ball(1) == 0 && centr_ball(2) == 0  
    % do nothing if ball is undetected
   else
   Dv = zeros(1,2);
   Dv(1)=centr_ball(1)-centr_drone(1); 
   Dv(2) = centr_ball(2) - centr_drone(2);
.
   %% set orientation to goal.
   %ath = atan2d(Dv(1,2),Dv(1,1)); %angle to horizon
   %The orientation of the drone is calibated using the yaw sensor
   %However, this fuction is unavailable due to a broken sensor
.
   %% Calling upon the drone's drive function
   % The speed of the drone is a linear function, saturated with a 
   % maximum speed for feasability and safety
.
          spRight   = 0;
          spBack    = 0;       
          max_speed = 0.4;  
.
           if Dv(1)>50
               spRight = max(Dv(1)*0.001*0.5,max_speed); end  %right
           if Dv(1)<-50
               spRight = min(Dv(1)*0.001*0.5,-max_speed); end %left
           if Dv(2)<-50
               spBack = max(-Dv(2)*0.001*0.5,max_speed); end  %forward
           if Dv(2)>50
               spBack = min(-Dv(2)*0.001*0.5,-max_speed); end %backward
.
   % for extra safety:
   % if the drone is near the boundaries of the field, it will thrust in the
   % opposite direction to slow itself down and quickly respond to the change
   if centr_drone(1)<300 spRight=0.5; end
   if centr_drone(1)>1500 spRight=-0.5; end 
   if centr_drone(2)<300 spBack=-0.5; end
   if centr_drone(2)>700 spBack=0.5; end
.
           drone.drive([-spBack,spRight,0,0]);
.
   end

end

linkje Matlabscript: link title


Terug naar: Control