function SetJointPositions( qf,JointSpeed )
%//////////////////////////////////////////////////////////////////////////
% @SERVO_MICROCONTROLLER (???Hz)
% Emulates the operation of the servomotors
%//////////////////////////////////////////////////////////////////////////
 global qi dqi CTRL_mode Ts NUM_samples JointSpeed
 
 if CTRL_mode == 2, NUM_samples = 5; end
 %disp('In----------------------------------------------------------------')
 %-- Trajectory planning --------------------------------------------------
  if abs(JointSpeed) < 1e-10, 
       Q = qi;
      dQ = [0 0 0]';
     ddQ = [0 0 0]';
     dqi = [0 0 0]';
  else
      for kk = 1:size(qf),
         % kk
         %-- Trajectory planning ------------------------------------------
          if abs(JointSpeed(kk)) > 1e-10,
             Texe = abs( (qf(kk)-qi(kk))/JointSpeed(kk) ); % Exec time
             t    = 0:Ts:Texe;                             % Time vector   
            %-- Polynomial trajectory generator ---------------------------
             switch CTRL_mode, 
             case 1,
             [Q(kk,:),dQ(kk,:),ddQ(kk,:)] = PolyTrajGenerator(t,t(end),...
                                           [qi(kk) qf(kk)],[dqi(kk) 0],[]);
             case 2,
             %NUM_samples,
             %qf(kk), qi(kk), JointSpeed(kk),
             %length(t)
             if sign(JointSpeed(kk)) ~= sign(dqi(kk)) & dqi(kk) ~= 0, 
                 %disp('olu---------------------------------------'), pause
                 dqi(kk) = 0;
             end
             [Q(kk,:),dQ(kk,:),ddQ(kk,:)] = PolyTrajGenerator(t(1:NUM_samples),t(end),...
                                           [qi(kk) qf(kk)],[dqi(kk) 0],[]);
             end
          else
              % disp('ole')
              [maxJointSpeed,I] = max( abs(JointSpeed) );
              Texe = abs( (qf(I)-qi(I)) )/maxJointSpeed; % Exec time
              DIM = length(0:Ts:Texe);                   % Time vector
                
              switch CTRL_mode, 
              case 1,
                Q(kk,:) = qi(1)*ones(1,DIM);
               dQ(kk,:) = zeros(1,DIM);
              ddQ(kk,:) = zeros(1,DIM);
              case 2,
                Q(kk,:) = qi(1)*ones(1,NUM_samples);
               dQ(kk,:) = zeros(1,NUM_samples);
              ddQ(kk,:) = zeros(1,NUM_samples);
              end
          end
      end
      %disp('end...............................................')
      %Q, dQ, ddQ
      switch CTRL_mode,          
          case 1, %-- Point-to-point motion
                   qi = qf;
                  dqi = [0 0 0]';
          case 2, %-- Continuous motion                 
                   qi = Q(:,NUM_samples);
                  dqi = dQ(:,NUM_samples);
          case 3, %-- COP-based motion control
      end
      
  end
 %-- Low-level motion control ---------------------------------------------
  Animation3D( Q );
 %----------------------------------------------------------------------end
end
