SCARA Vismach - Gcode error "exceed C's positive limit"

More
18 Sep 2024 10:22 #310360 by DPFlex
Hello,
My system is:
- LCNC 2.9.3, Debian 6.1.0-22-rt-amd64
- EtherCAT AC servo
- Everything is ok: homing, moving each axis separately, ...

When I run G-code, I have strange error: "Linear move on line xxx would exceed C's positive/negative limit".
I remember that it was ok with LCNC 2.9.2, but I have trouble with LCNC 2.9.3
I did big number of MIN & MAX Limit, but could not run G-Code.
Any advise ?. Thank you.
 
 
 

File Attachment:

File Name: tsino_scara_3ax.ini
File Size:3 KB

 

File Attachment:

File Name: vismach_au...ABv1.ngc
File Size:0 KB


 
Attachments:

Please Log in or Create an account to join the conversation.

  • Aciera
  • Aciera's Avatar
  • Away
  • Administrator
  • Administrator
More
18 Sep 2024 11:55 #310365 by Aciera
I'm not familiar with 'scarakins' but looking at 'configs/sim/axis/vismach/scara/scara.ini' and comparing that with your 'tsino_scara_3ax.ini' it seems to me that you have not configured enough joints:
[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

Please Log in or Create an account to join the conversation.

More
18 Sep 2024 13:52 #310367 by DPFlex
Hi Aciera,
Yes, correct, I copied scara.ini from vismach demo and revised it.
Actually there are 3 joints (J0, J1, J2) which are most useful in SCARA application. That's why I only test with 3 joints and removed 3 joints (J3,J4,J5) :)
Is it root cause to have this error ?. should it be ?.
Thank you.

Please Log in or Create an account to join the conversation.

  • Aciera
  • Aciera's Avatar
  • Away
  • Administrator
  • Administrator
More
18 Sep 2024 15:51 #310374 by Aciera
Problem is that the kinematic model in scarakins.c is for a scara with 3 angular joints and a linear joint and also two angular joints that move the table. You could easily remove those last two (ie j4/a and j5/b) from the kinematic as they are just looped through. J3/c however is part of the calculation so to remove that you will need to have a closer look at the forward and inverse kinematic model.
/* 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()
The following user(s) said Thank You: DPFlex

Please Log in or Create an account to join the conversation.

More
19 Sep 2024 12:46 #310449 by DPFlex
Hi Aciera,
You are correct. I added joint-3 and it worked :)

I see another issue of "tk window".
As my testing, I test moving from position-A to position-B in 200 times.
Normally after 50th time, the "tk window" is freezing, although G-code and real motors are still running.

Have you faced this issue ?.
Thank you.

Please Log in or Create an account to join the conversation.

  • Aciera
  • Aciera's Avatar
  • Away
  • Administrator
  • Administrator
More
19 Sep 2024 13:26 #310453 by Aciera
Try starting your config through a terminal
$ linuxcnc

and see if you get any errors in the terminal output when the tk window freezes.

Please Log in or Create an account to join the conversation.

More
19 Sep 2024 21:48 #310481 by raf1110
You can change debug level to 5 i think it will give more details if you run from terminal. ;)
 

Please Log in or Create an account to join the conversation.

Time to create page: 0.100 seconds
Powered by Kunena Forum