Generation and Control of Basic Geometric Trajectories for a Robot Manipulator Using CompactRIO®

Algorithm 3

Algorithm for joint movement of MOVEC.

         Data: ,

         Result: Interpolated positions

      Vectors and ;

       Value in associated to the system to ;

      ;

       Vector normal to the plane or associated to

           the system to ;

       Value in associated with the system to ;

       Angle between

           vectors and ;

      ;

      ;

      ;

   ;

    of the center of the circumference;

    of the center of the

           circumference;

    ;

   ;

   ;

    interpolated from the circumference;

    interpolated from the circumference;

   ;

   for    do

      ;

      ;

      ;

      ;

      ;

       Inverse Kinematic Model

   end

   Interpolated positions ;