Matlabscript TopCamera

From Control Systems Technology Group
Jump to navigation Jump to search
The printable version is no longer supported and may have rendering errors. Please update your browser bookmarks and please use the default browser print function instead.

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: All-Project Matlab Files


Terug naar: Control