Matlabscript TopCamera

From Control Systems Technology Group

Jump to: navigation, 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
   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

Personal tools