Matlabscript TopCamera

From Control Systems Technology Group
Jump to navigation Jump to search

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
   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[-spBack,spRight,0,0]);
end end

linkje Matlabscript: All-Project Matlab Files

Terug naar: Control