View source for Matlabscript TopCamera

<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  
     % 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
</code>

linkje Matlabscript: [http://www.example.com link title]

----

Terug naar: [[Control]]