Advanced Search

/* joint[0], joint[1] and joint[3] are in degrees and joint[2] is in length units */
static
int scaraKinematicsForward(const double * joint,
                      EmcPose * world,
                      const KINEMATICS_FORWARD_FLAGS * fflags,
                      KINEMATICS_INVERSE_FLAGS * iflags)
{
    double a0, a1, a3;
    double x, y, z, c;

/* convert joint angles to radians for sin() and cos() */

    a0 = joint[0] * ( PM_PI / 180 );
    a1 = joint[1] * ( PM_PI / 180 );
    a3 = joint[3] * ( PM_PI / 180 );
/* convert angles into world coords */

    a1 = a1 + a0;
    a3 = a3 + a1;

    x = D2*cos(a0) + D4*cos(a1) + D6*cos(a3);
    y = D2*sin(a0) + D4*sin(a1) + D6*sin(a3);
    z = D1 + D3 - joint[2] - D5;
    c = a3;

    *iflags = 0;
    if (joint[1] < 90)
        *iflags = 1;

    world->tran.x = x;
    world->tran.y = y;
    world->tran.z = z;
    world->c = c * 180 / PM_PI;

    world->a = joint[4];
    world->b = joint[5];

    return (0);
} //scaraKinematicsForward()

static int scaraKinematicsInverse(const EmcPose * world,
                                  double * joint,
                                  const KINEMATICS_INVERSE_FLAGS * iflags,
                                  KINEMATICS_FORWARD_FLAGS * fflags)
{
    double a3;
    double q0, q1;
    double xt, yt, rsq, cc;
    double x, y, z, c;

    x = world->tran.x;
    y = world->tran.y;
    z = world->tran.z;
    c = world->c;

    /* convert degrees to radians */
    a3 = c * ( PM_PI / 180 );

    /* center of end effector (correct for D6) */
    xt = x - D6*cos(a3);
    yt = y - D6*sin(a3);

    /* horizontal distance (squared) from end effector centerline
        to main column centerline */
    rsq = xt*xt + yt*yt;
    /* joint 1 angle needed to make arm length match sqrt(rsq) */
    cc = (rsq - D2*D2 - D4*D4) / (2*D2*D4);
    if(cc < -1) cc = -1;
    if(cc > 1) cc = 1;
    q1 = acos(cc);

    if (*iflags)
        q1 = -q1;

    /* angle to end effector */
    q0 = atan2(yt, xt);

    /* end effector coords in inner arm coord system */
    xt = D2 + D4*cos(q1);
    yt = D4*sin(q1);

    /* inner arm angle */
    q0 = q0 - atan2(yt, xt);

    /* q0 and q1 are still in radians. convert them to degrees */
    q0 = q0 * (180 / PM_PI);
    q1 = q1 * (180 / PM_PI);

    joint[0] = q0;
    joint[1] = q1;
    joint[2] = D1 + D3 - D5 - z;
    joint[3] = c - ( q0 + q1);
    joint[4] = world->a;
    joint[5] = world->b;

    *fflags = 0;

    return (0);
} // scaraKinematicsInverse()

[EMC]
VERSION = 1.1
MACHINE = SCARA (genserkins,switchkins)
#+ Debug level, 0 means no messages. See src/emc/nml_int/emcglb.h for others
#DEBUG = 0

[DISPLAY]
DISPLAY = axis
CYCLE_TIME = 0.200
POSITION_OFFSET = RELATIVE
POSITION_FEEDBACK = ACTUAL
MAX_FEED_OVERRIDE = 2.0
PROGRAM_PREFIX = ../../nc_files/
INTRO_GRAPHIC = linuxcnc.gif
INTRO_TIME = 5
PYVCP = scara.xml
#EDITOR = geany

[RS274NGC]
SUBROUTINE_PATH = ./remap_subs
   HAL_PIN_VARS = 1
   HAL_PIN_VARS = 1
          REMAP = M428  modalgroup=10  ngc=428remap
          REMAP = M429  modalgroup=10  ngc=429remap
          REMAP = M430  modalgroup=10  ngc=430remap
PARAMETER_FILE = scara.var
# startup mm and offsets for axis.ngc demo file:
RS274NGC_STARTUP_CODE = G21 G10L2P0 x432 y65 z416 (debug, ini: startup offsets)

[EMCMOT]
EMCMOT = motmod
COMM_TIMEOUT = 1.0
SERVO_PERIOD = 1000000

[TASK]
TASK = milltask
CYCLE_TIME = 0.010

[KINS]
JOINTS = 6
# switchkins:
#   kinstype==0 (default scarakins)
#     j0,j1,j2,j3 -- implement scarakins for xyzc
#     j4 == world->a (scaragui j4 is table tilt (y))
#     j5 == world->b (scaragui j5 is table tilt (x))
#   kinstype==1 (identity, assign per coordinates=xyzcab)
#     letter x -- assigned to j0
#     letter y -- assigned to j1
#     letter z -- assigned to j2
#     letter c -- assigned to j3
#     letter a -- assigned to j4 (same as scarakins)
#     letter b -- assigned to j5 (same as scarakins)
#   kinstype==2 (userk, template is identity, user may specify)
#
# Note: coordinate= ordering for identity kins preserves ab relationships
KINEMATICS = scarakins coordinates=xyzcab

[HAL]
HALUI   = halui
HALFILE = LIB:basic_sim.tcl
HALCMD  = loadusr -W scaragui
HALCMD  = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type
POSTGUI_HALFILE = scara_postgui.hal

[HALUI]
# M428:scara kins    (kinstype==0 startupDEFAULT)
# M429:identity kins (kinstype==1)
# M430:userk kins    (kinstype==2)
MDI_COMMAND = M428
MDI_COMMAND = M429
MDI_COMMAND = M430

[TRAJ]
COORDINATES = XYZABC
LINEAR_UNITS = mm
DEFAULT_LINEAR_VELOCITY = 10.0
MAX_LINEAR_VELOCITY = 600.0
DEFAULT_LINEAR_ACCELERATION = 100.0
MAX_LINEAR_ACCELERATION = 200.0
ANGULAR_UNITS = degree
DEFAULT_ANGULAR_VELOCITY = 10.0
MAX_ANGULAR_VELOCITY = 600.0
DEFAULT_ANGULAR_ACCELERATION = 100.0
MAX_ANGULAR_ACCELERATION = 200.0

[EMCIO]
EMCIO = io
CYCLE_TIME = 0.100
TOOL_TABLE = scara.tbl

[AXIS_X]
MAX_VELOCITY = 30.0
MAX_ACCELERATION = 200.0
[AXIS_Y]
MAX_VELOCITY = 30.0
MAX_ACCELERATION = 200.0
[AXIS_Z]
MAX_VELOCITY = 30.0
MAX_ACCELERATION = 200.0
[AXIS_C]
MAX_VELOCITY = 60.0
MAX_ACCELERATION = 400.0

[JOINT_0]
NAME = shoulder
TYPE = ANGULAR
MAX_VELOCITY = 30.0
MAX_ACCELERATION = 200.0
MIN_LIMIT = -360.0
MAX_LIMIT = 360.0
HOME_OFFSET = 0.0
HOME = 0.000
HOME_SEQUENCE = 0

[JOINT_1]
NAME = elbow
TYPE = ANGULAR
MAX_VELOCITY = 30.0
MAX_ACCELERATION = 200.0
MIN_LIMIT = -360.0
MAX_LIMIT = 360.0
HOME_OFFSET = 0.0
HOME = 0.000
HOME_SEQUENCE = 0

[JOINT_2]
NAME = z-slide
TYPE = LINEAR
MAX_VELOCITY = 30.0
MAX_ACCELERATION = 200.0
MIN_LIMIT = 25.0
MAX_LIMIT = 300.0
HOME_OFFSET = 25.0
HOME = 25.000
HOME_SEQUENCE = 0

[JOINT_3]
NAME = wrist
TYPE = ANGULAR
MAX_VELOCITY = 60.0
MAX_ACCELERATION = 400.0
MIN_LIMIT = -180.0
MAX_LIMIT = 180.0
HOME_OFFSET = 0.0
HOME = 0.000
HOME_SEQUENCE = 0

[JOINT_4]
NAME = table_rotate
TYPE = ANGULAR
MAX_VELOCITY = 60.0
MAX_ACCELERATION = 400.0
MIN_LIMIT = -180.0
MAX_LIMIT =  180.0
HOME_OFFSET = 0.0
HOME = 0.000
HOME_SEQUENCE = 0

[JOINT_5]
NAME = table_rotate
TYPE = ANGULAR
MAX_VELOCITY = 60.0
MAX_ACCELERATION = 400.0
MIN_LIMIT = -180.0
MAX_LIMIT =  180.0
HOME_OFFSET = 0.0
HOME = 0.000
HOME_SEQUENCE = 0