Program Examples

Custom Kinematic Transformations

Another useful feature built-in to all Trio controllers is the ability to do coordinate transformations, allowing the motion to be programmed with traditional TrioBASIC instructions, using the standard coordinate system, whilst the controller controls the mechanical system to achieve the programmed moves in the transformed coordinate system.

A good example of a kinematic transformation is a Polar robot. If the user wants to program in the traditional XY coordinate system, the controller must perform the translation to radius (R) and angle (θ) for the mechanics to move to this position.

 

 

There are normally two functional parts to a transformation:

1. The forward transformation
The forward transformation is as discussed above, based on the XY coordinates in the normal coordinate system, this gives the current target position for the axes in the target coordinate system.

2. The inverse or reverse transformation
The reverse transformation allows the XY position to be determinedby taking feedback of currentposition of themechanics an performing an inverse transformation.

Trio supports a number of standard coordinate transformations, and these are setup using the frame command. A machine system can be specified with several different "frames". The default FRAME is 0

---

FRAME=0-NORMAL TRANSLATION

This is the default transformation one-to-one frame transformation with FRAME=0 for a conventional X-Y system. In this mode, an interpolated move of MOVE(0,100) produces motion on the Y-motor only to raise the load vertically. Note that the weight of the Y-motor must be carried by the X-motor.

---

FRAME=1-2 axis Scara

This transformation sets up a scara robot. It is setup by putting the following data into table locations:

Table Location	Value
0	Link 1 length (microns)
1	Link 2 length (microns)
2	Encoder edges/radian on angle t1
3	Encoder edges/radian on angle t2

---

FRAME=2-Single Belt XY robot

This sets up the controller to control a single belt robot as shown in the picture below. In this mode, an interpolated move of MOVE(0,100) produces motion on both motor1 and motor2 to raise the load vertically, based on the transformed position. Note that the two motors are located on the X-axis. The mass of the Y-axis can be minimised in this configuration. See TN30-12 for further details.

---

FRAME=3 Single Belt multiple

As frame 2 but allows a second XY to be configured on axis 3 & 4. Please note that all 4 axes must be datumed before the frame command can be started.

---

FRAME=4

2 Axes Pick and Place frame Frame 4 is designed for 2 physical axes that are positioned at 90 degrees to one another, X and Z. The arrangement is such that there is interaction between the 2 axes when seen at the tool tip. Frame 4 mathematically transforms this geometry to standard x / z coordinates. See tech note TN20-42 for more details.

---

FRAME=5

This frame is designed to allow axes 0 and 1 to be “turned” through an angle so that command inputs to x, y (along the required plane) are transformed to the fixed axes x’ and y’. The transform is done by way of a 2 x 2 matrix, the coefficients of which can be easily derived from the required rotation angle of the operating plane. See tech note TN20-37 for more details.

---

FRAME=10

Polar This transformation takes X and Y Cartesian coordinates and produces the equivalent R and θ polar values. All interpolated and single axis moves passed to axes 0 and 1 are followed by those 2 axes working in the polar domain. Axis 0 controls movement along the radius. Axis 1 controls the angle theta. Axis 1 positions are held internally as integers and the axis is calibrated by entering the number of counts per radian into TABLE point 0.

---

FRAME=? Customised transformations

Customised frame transformations can be added to the controller to perform additional transformations to suit your exact needs. In this case the transformation is written in ‘C’, compiled and added into the controller system software. As an example, we have a frame transform that will be activated using the FRAME=20 command, which is selected from the CASE statement as highlighted. In this example the current positions are doubled for the forward transformation and the reverse transformation will halve the current positions.

Forward Transformation

The forward transformation takes the DPOS position as calculated by the controller trajectory generator, doubles the value and updates TRANS_POS, which is used as the DPOS during frame transformations.

The following code realises the above transformation, and is shown as red in the program: axis[0].i_val[TRANS_DPOS]=axis[0].i_val[DEMAND_POS]*2; axis[1].i_val[TRANS_DPOS]=axis[0].i_val[DEMAND_POS]*2;

Reverse Transformation

The reverse transformation takes the TRANS_DPOS position as measured by the controller axis feedback and divides the value by 2 and updates DEMAND_POS which is the DPOS reported in the controller.

The following code realises the above transformation, and is shown as red in the program: axis[0].i_val[DEMAND_POS]=axis[0].i_val[ENDMOVE]=axis[0].i_val[TRANS_DPOS]/2; axis[1].i_val[DEMAND_POS]=axis[1].i_val[ENDMOVE]=axis[0].i_val[TRANS_DPOS]/2;


/***********************************************
*  Module: Coordinate transformation function  *
*              File: TRANSFRM.C                *
************************************************/
#include "basic.h"
#include "constant.h"
public void transform()
{
 /* local variables */
 int ax;
  /* if frame has changed then process it */
  if (common.servo.i_val[FRAME] != common.servo.i_val[PREV_FRAME])
  { 
   /* frame has changed: do reverse transform from current frame number*/
    switch ( common.servo.i_val[FRAME])
   {
    case 20:
     /* Reverse Transformation */
     axis[0].i_val[DEMAND_POS]=axis[0].i_val[ENDMOVE]=axis[0].i_val[TRANS_DPOS]/2;      axis[1].i_val[DEMAND_POS]=axis[1].i_val[ENDMOVE]=axis[0].i_val[TRANS_DPOS]/2;
      /* Leave AXES 2+ untouched */
      for(ax=2;ax<AXES;ax++)
       {
        axis[ax].i_val[DEMAND_POS]=axis[ax].i_val[TRANS_DPOS];
        axis[ax].i_val[ENDMOVE]=axis[ax].i_val[DEMAND_POS];
       }
      break;

      case 0: default:
       for(ax=0;ax<AXES;ax++)
        {
         axis[ax].i_val[DEMAND_POS]=axis[ax].i_val[TRANS_DPOS];
         axis[ax].i_val[ENDMOVE]=axis[ax].i_val[DEMAND_POS];
        }
       break;
      }
       /* Now set current frame= new frame */
       common.servo.i_val[PREV_FRAME]=common.servo.i_val[FRAME];
      }
      /* Now do forward transformation routine for current frame: */
      switch(common.servo.i_val[PREV_FRAME])
     {
     case 20:
      /* Forward Transformation */
      axis[0].i_val[TRANS_DPOS]=axis[0].i_val[DEMAND_POS]*2;       axis[1].i_val[TRANS_DPOS]=axis[0].i_val[DEMAND_POS]*2;
     /* Leave AXES 2..11 untouched */
     for(ax=2;ax<AXES;ax++) { axis[ax].i_val[TRANS_DPOS]=axis[ax].i_val[DEMAND_POS];
     }
   break;
  case 0: default:
  for(ax=0;ax<AXES;ax++)
  {
 axis[ax].i_val[TRANS_DPOS]=axis[ax].i_val[DEMAND_POS];
 }
 break;
 }
}

Contact Trio if you need to do this.