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)