function SetJointPositions( qf,JointSpeed )
%//////////////////////////////////////////////////////////////////////////
% @SERVO_MICROCONTROLLER (???Hz)
% Emulates the operation of the servomotors
%//////////////////////////////////////////////////////////////////////////
 
 global L R M CM0x CM0y go
 global ANG dANG ddANG COP COG GRF T tt
 global qi dqi M_01_i CTRL_mode Ts NUM_samples JointSpeed                                                          
 
 SIM_servo = 1;
 if CTRL_mode == 2 || CTRL_mode == 3, 
    if SIM_servo, NUM_samples = 5; else NUM_samples = 1; end
 end
 %disp('In---------------------------------------------------------------')
 %-- Trajectory planning --------------------------------------------------
  if abs(JointSpeed) < 1e-10, 
      dqi = [0 0 0]';
     ddqi = [0 0 0]';
     if CTRL_mode == 1,
          Q = qi;
         dQ = [0 0 0]';
        ddQ = [0 0 0]';         
     else
         Q = repmat(qi,1,NUM_samples);
           ANG = Q;
          dANG = repmat([0 0 0]',1,NUM_samples);
         ddANG = repmat([0 0 0]',1,NUM_samples);
         COG = repmat(RobCOG( qi,L,R,M ),1,NUM_samples);
         COP = COG(1:2,:);
         A = zeros(3,3);
         [M_01_i,T_i] = InvDyn3dof( A,qi,L,R,M,go );
         T = repmat(T_i,1,NUM_samples);
         GRF = repmat(RobGRF( A,M,go ),1,NUM_samples);
         tt = tt+NUM_samples;
     end
  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
            %-- Polynomial trajectory generator ---------------------------
             switch CTRL_mode, 
             case 1,
             t    = 0:Ts:Texe;    
             [Q(kk,:),dQ(kk,:),ddQ(kk,:)] = PolyTrajGenerator(t,t(end),...
                                           [qi(kk) qf(kk)],[dqi(kk) 0],[]);
             case {2,3},
                 if ~SIM_servo,
                    dQ(kk,1:NUM_samples) = JointSpeed(kk);
                    for ii = 1:NUM_samples,
                        Q(kk,ii) = qi(kk)+JointSpeed(kk)*Ts;
                        qi(kk) = Q(kk,ii);
                    end
                    ddQ(kk,1) = ( JointSpeed(kk)- dqi(kk) )/Ts;
                    ddQ(kk,2:NUM_samples) = 0;
                 else
                     t    = 0:Ts:(NUM_samples-1)*Ts;
%                      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),Texe,...
                                           [qi(kk) qf(kk)],[dqi(kk) 0],[]);
                 end
             end
          else
              % disp('ole')                
              switch CTRL_mode, 
              case 1,
                  [maxJointSpeed,I] = max( abs(JointSpeed) );
              Texe = abs( (qf(I)-qi(I)) )/maxJointSpeed; % Exec time
              DIM = length(0:Ts:Texe);                   % Time vector
                Q(kk,:) = qi(kk)*ones(1,DIM);
               dQ(kk,:) = zeros(1,DIM);
              ddQ(kk,:) = zeros(1,DIM);
              case {2,3},
                Q(kk,:) = qi(kk)*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]';
                  ddqi = [0 0 0]';
                  for ii = 1:size(Q,2),
                      A = LinAce3dof_COG( Q(:,ii),dQ(:,ii),ddQ(:,ii),L,R );% Linear CM accel
                      [M_01_i,T(:,ii)] = InvDyn3dof( A,Q(:,ii),L,R,M,go ); % Moments 
                      GRF(:,ii) = RobGRF( A,M,go );   % Read force sensors
                      COP(:,ii) = RobCOP( M_01_i,GRF(:,ii),M(1),CM0x,CM0y,L(1),go ); % Compute COP
                      COG(:,ii) = RobCOG( Q(:,ii),L,R,M );
                  end
                  T2_max = max(abs(T(2,:))), T2_max = max(abs(T(3,:))) 
                  PlotGraphics( t,Q,dQ,T,GRF,COP,COG );
          case {2,3}, %-- Continuous motion  
                  for ii = 1:size(Q,2),
                      tt = tt+1;   % Incremental timer -------------------
                        ANG(:,tt) = Q(:,ii);
                       dANG(:,tt) = dQ(:,ii);
                      ddANG(:,tt) = ddQ(:,ii);
                      A = LinAce3dof_COG( Q(:,ii),dQ(:,ii),ddQ(:,ii),L,R );% Linear CM accel
                      [M_01_i,T(:,tt)] = InvDyn3dof( A,Q(:,ii),L,R,M,go ); % Moments 
                      %A, M_01_i, pause
                      GRF(:,tt) = RobGRF( A,M,go );   % Read force sensors
                      COP(:,tt) = RobCOP( M_01_i,GRF(:,tt),M(1),CM0x,CM0y,L(1),go ); % Compute COP
                      %COP(:,tt)
                      COG(:,tt) = RobCOG( Q(:,ii),L,R,M );
                  end                       
                    qi = Q(:,NUM_samples);
                   dqi = dQ(:,NUM_samples);
                  ddqi = ddQ(:,NUM_samples);
                  A = LinAce3dof_COG( qi,dqi,ddqi,L,R );  % Linear CM accel
                  [M_01_i,T_i] = InvDyn3dof( A,qi,L,R,M,go ); % Moments          
      end
      
  end
 %-- Low-level motion control ---------------------------------------------
  Animation3D( Q,COG(:,end-NUM_samples+1:end),COP(:,end-NUM_samples+1:end) );
 %----------------------------------------------------------------------end
end
