Matlabscript TopCamera

Hieronder staat de Matlab-code voor de top camera. Comments over gebruikte code staan in het script erbij vermeldt.

 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