Delay Between Simulated Servo Speed and Real Time Servo Speed (using tic/toc)

I am currently trying run six servos (DS3218 20kg cm) using the Arduino MEGA rev3. The servos follow a trajectory path given a position and time input i.e. they go to a certain position at a certain time, or are supposed to at least. The time vector that supplies the joint kinematics goes from 0 to 0.4999 seconds; however, when implementing tic and toc, it can be seen that it actually takes about 23 seconds to implement. At first I thought it was that the Arduino was not fast enough, and it still might be the case, but after using the "profile" in matlab with "Run and Time" I found that using servo writePosition was taking 26.14 seconds to execute. Closer inspection showed that a specific line in the code rawWrite takes about 24 seconds. The goal was to have it run in real time and match the trajectory to the 0.4999 seconds. Would this not be possible because the board is too slow otherwise I am not sure how to increase the execution speed of a built-in matlab function? Let me know if anything needs clarified. Thank you in advance for any suggestions.

motor_min=0*(pi()/180); %motor pulley angle deg
motor_max=162*(pi()/180); %deg
anglefinger_min=0*(pi()/180); %finger angle
anglefinger_max=90*(pi()/180); 
motor_minservo=0.1; %servo pulley angle
motor_maxservo=0.7;
t=pos.tTime; %vector of positions going from 0 to 0.4999 by 0.001
position=pos.ra_mcp2_e_f; %angular positions going from 0 to 1.57 in radians

time=1;
% toc=0.002;
anglefinger=zeros(1,500);
angleservo=zeros(1,500);
angleservo_input=zeros(1,500);
trackedtime=zeros(1,500);
while time<=length(t)
    tic;
    anglefinger(time)=pos.ra_mcp2_e_f(time); %(0-80deg)finger and (0-162)servo
    angleservo(time)=motor_min+(((motor_max-motor_min)/(anglefinger_max-anglefinger_min))*(anglefinger(time)-anglefinger_min));
    angleservo_input(time)=round(motor_minservo+(((motor_maxservo-motor_minservo)/(motor_max-motor_min))*(angleservo(time)-motor_min)),2,'significant');
    move_fingers_new(s1,s2,s3,s4,s5,s6,angleservo_input(time),0.1,0.1,0.1,0.5,0.5);
    trackedtime(time)=toc;
    time=time+1;
end

Code used for the move_fingers_new function above

function move_fingers_new(s1,s2,s3,s4,s5,s6,angle1,angle2,angle3,angle4,angle5,angle6)
% writePosition(s(1), angle(1));
% writePosition(s(2), angle(2));
% writePosition(s(3), angle(3));
% writePosition(s(4), angle(4));
% writePosition(s(5), angle(5));
% writePosition(s(6), angle(6));
% current_pos = readPosition(s);
% current_pos = current_pos*180;
% fprintf('Current motor position is %d degrees\n', current_pos)
writePosition(s1, angle1);
writePosition(s2, angle2);
writePosition(s3, angle3);
writePosition(s4, angle4);
writePosition(s5, angle5);
writePosition(s6, angle6);

Matlab_Code_Time_rawWrite.pdf (339 KB)

Matlab_Code_TimeTotal.pdf (230 KB)

Closer inspection showed that a specific line in the code rawWrite takes about 24 seconds.

Glad that you know which line it is. Can you share that information with us?

It is one of the attached documents, but here it is as well. The line rawWrite is taking about 24 seconds to execute.

          function writePosition(obj, value)
            %   Set the position of servo motor shaft.
            %
            %   Syntax:
            %   writePosition(s, value)
            %
            %   Description:
            %   Set the position of a standard servo motor shaft as a
            %   ratio of the motor's min/max range, from 0 to 1
            %
            %   Example:
            %       a = arduino();
            %       s = servo(a, 'D9');
            %       writePosition(s, 0.60);
			%
            %   Example:
            %       a = arduino();
            %       dev = addon(a, 'Adafruit/MotorShieldV2');
            %       s = servo(dev,'D9');
            %       writePosition(s, 0.60);
			%
            %   Input Arguments:
            %   s       - Servo motor device 
            %   value   - Motor shaft position (double)
			%
			%   See also readPosition
            
            try
                if (nargin < 2)
                    obj.localizedError('MATLAB:minrhs');
                end
                matlabshared.hwsdk.internal.validateDoubleParameterRanged('position', value, 0, 1);
                peripheralPayload = [getPinNumber(obj.Parent, obj.Pin) uint8(180*value)];
                rawWrite(obj.Parent.Protocol, obj.WRITE_POSITION, peripheralPayload);
            catch e
                throwAsCaller(e);
            end
        end
    end