command.c

Source code for an Numeric Cmputer

	    for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
		/* point to joint struct */
		joint = &joints[joint_num];
		/* update status flags */
		SET_JOINT_ERROR_FLAG(joint, 0);
		SET_JOINT_FAULT_FLAG(joint, 0);
	    }
	    break;

	case EMCMOT_AXIS_ABORT:
	    /* abort one axis */
	    /* can happen at any time */
	    /* this command stops a single axis.  It is only usefull
	       in free mode, so in coord or teleop mode it does
	       nothing. */
	    rtapi_print_msg(RTAPI_MSG_DBG, "AXIS_ABORT");
	    rtapi_print_msg(RTAPI_MSG_DBG, " %d", emcmotCommand->axis);
	    if (GET_MOTION_TELEOP_FLAG()) {
		/* do nothing in teleop mode */
	    } else if (GET_MOTION_COORD_FLAG()) {
		/* do nothing in coord mode */
	    } else {
		/* validate joint */
		if (joint == 0) {
		    break;
		}
		/* tell joint planner to stop */
		joint->free_tp_enable = 0;
		/* stop homing if in progress */
		if ( joint->home_state != HOME_IDLE ) {
		    joint->home_state = HOME_ABORT;
		}
		/* update status flags */
		SET_JOINT_ERROR_FLAG(joint, 0);
	    }
	    break;

	case EMCMOT_FREE:
	    /* change the mode to free axis motion */
	    /* can be done at any time */
	    /* this code doesn't actually make the transition, it merely
	       requests the transition by clearing a couple of flags */
	    /* reset the emcmotDebug->coordinating flag to defer transition
	       to controller cycle */
	    rtapi_print_msg(RTAPI_MSG_DBG, "FREE");
	    emcmotDebug->coordinating = 0;
	    emcmotDebug->teleoperating = 0;
	    break;

	case EMCMOT_COORD:
	    /* change the mode to coordinated axis motion */
	    /* can be done at any time */
	    /* this code doesn't actually make the transition, it merely
	       tests a condition and then sets a flag requesting the
	       transition */
	    /* set the emcmotDebug->coordinating flag to defer transition to
	       controller cycle */
	    rtapi_print_msg(RTAPI_MSG_DBG, "COORD");
	    emcmotDebug->coordinating = 1;
	    emcmotDebug->teleoperating = 0;
	    if (kinType != KINEMATICS_IDENTITY) {
		if (!checkAllHomed()) {
		    reportError
			("all axes must be homed before going into coordinated mode");
		    emcmotDebug->coordinating = 0;
		    break;
		}
	    }
	    break;

	case EMCMOT_TELEOP:
	    /* change the mode to teleop motion */
	    /* can be done at any time */
	    /* this code doesn't actually make the transition, it merely
	       tests a condition and then sets a flag requesting the
	       transition */
	    /* set the emcmotDebug->teleoperating flag to defer transition to
	       controller cycle */
	    rtapi_print_msg(RTAPI_MSG_DBG, "TELEOP");
	    emcmotDebug->teleoperating = 1;
	    if (kinType != KINEMATICS_IDENTITY) {
		
		if (!checkAllHomed()) {
		    reportError
			("all axes must be homed before going into teleop mode");
		    emcmotDebug->teleoperating = 0;
		    break;
		}

	    }
	    break;

	case EMCMOT_SET_NUM_AXES:
	    /* set the global NUM_AXES, which must be between 1 and
	       EMCMOT_MAX_AXIS, inclusive */
	    /* this sets a global - I hate globals - hopefully this can be
	       moved into the config structure, or dispensed with completely */
	    rtapi_print_msg(RTAPI_MSG_DBG, "SET_NUM_AXES");
	    joint_num = emcmotCommand->axis;
	    rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
	    /* note that this comparison differs from the check on the range
	       of 'joint_num' in most other places, since those checks are
	       for a value to be used as an index and here it's a value to be 
	       used as a counting number. The indenting is different here so
	       as not to match macro editing on that other bunch. */
	    if (joint_num <= 0 || joint_num > EMCMOT_MAX_AXIS) {
		break;
	    }
	    num_axes = joint_num;
	    emcmotConfig->numAxes = joint_num;
	    break;

	case EMCMOT_SET_WORLD_HOME:
	    rtapi_print_msg(RTAPI_MSG_DBG, "SET_WORLD_HOME");
	    emcmotStatus->world_home = emcmotCommand->pos;
	    break;

	case EMCMOT_SET_HOMING_PARAMS:
	    rtapi_print_msg(RTAPI_MSG_DBG, "SET_HOMING_PARAMS");
	    rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
	    emcmot_config_change();
	    if (joint == 0) {
		break;
	    }
	    joint->home_offset = emcmotCommand->offset;
	    joint->home = emcmotCommand->home;
	    joint->home_search_vel = emcmotCommand->search_vel;
	    joint->home_latch_vel = emcmotCommand->latch_vel;
	    joint->home_flags = emcmotCommand->flags;
	    break;

	case EMCMOT_OVERRIDE_LIMITS:
	    /* this command can be issued with axix < 0 to re-enable
	       limits, but they are automatically re-enabled at the
	       end of the next jog */
	    rtapi_print_msg(RTAPI_MSG_DBG, "OVERRIDE_LIMITS");
	    rtapi_print_msg(RTAPI_MSG_DBG, " %d", emcmotCommand->axis);
	    if (emcmotCommand->axis < 0) {
		/* don't override limits */
		rtapi_print_msg(RTAPI_MSG_DBG, "override off");
		emcmotStatus->overrideLimits = 0;
	    } else {
		rtapi_print_msg(RTAPI_MSG_DBG, "override on");
		emcmotStatus->overrideLimits = 1;
	    }
	    emcmotDebug->overriding = 0;
	    for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
		/* point at joint data */
		joint = &joints[joint_num];
		/* clear joint errors */
		SET_JOINT_ERROR_FLAG(joint, 0);
	    }
	    break;

	case EMCMOT_SET_POSITION_LIMITS:
	    /* sets soft limits for an axis */
	    rtapi_print_msg(RTAPI_MSG_DBG, "SET_POSITION_LIMITS");
	    rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
	    emcmot_config_change();
	    /* set the position limits for the axis */
	    /* can be done at any time */
	    if (joint == 0) {
		break;
	    }
	    joint->min_pos_limit = emcmotCommand->minLimit;
	    joint->max_pos_limit = emcmotCommand->maxLimit;
	    break;

	case EMCMOT_SET_BACKLASH:
	    /* sets backlash for an axis */
	    rtapi_print_msg(RTAPI_MSG_DBG, "SET_BACKLASH");
	    rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
	    emcmot_config_change();
	    /* set the backlash for the axis */
	    /* can be done at any time */
	    if (joint == 0) {
		break;
	    }
	    joint->backlash = emcmotCommand->backlash;
	    break;

	    /*
	       Max and min ferror work like this: limiting ferror is
	       determined by slope of ferror line, = maxFerror/limitVel ->
	       limiting ferror = maxFerror/limitVel * vel. If ferror <
	       minFerror then OK else if ferror < limiting ferror then OK
	       else ERROR */
	case EMCMOT_SET_MAX_FERROR:
	    rtapi_print_msg(RTAPI_MSG_DBG, "SET_MAX_FERROR");
	    rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
	    emcmot_config_change();
	    if (joint == 0 || emcmotCommand->maxFerror < 0.0) {
		break;
	    }
	    joint->max_ferror = emcmotCommand->maxFerror;
	    break;

	case EMCMOT_SET_MIN_FERROR:
	    rtapi_print_msg(RTAPI_MSG_DBG, "SET_MIN_FERROR");
	    rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
	    emcmot_config_change();
	    if (joint == 0 || emcmotCommand->minFerror < 0.0) {
		break;
	    }
	    joint->min_ferror = emcmotCommand->minFerror;
	    break;

	case EMCMOT_JOG_CONT:
	    /* do a continuous jog, implemented as an incremental jog to the
	       limit.  When the user lets go of the button an abort will
	       stop the jog. */
	    rtapi_print_msg(RTAPI_MSG_DBG, "JOG_CONT");
	    rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
	    /* check axis range */
	    if (joint == 0) {
		break;
	    }

	    /* must be in free mode and enabled */
	    if (GET_MOTION_COORD_FLAG()) {
		reportError("Can't jog axis in coordinated mode.");
		SET_JOINT_ERROR_FLAG(joint, 1);
		break;
	    }
	    if (!GET_MOTION_ENABLE_FLAG()) {
		reportError("Can't jog axis when not enabled.");
		SET_JOINT_ERROR_FLAG(joint, 1);
		break;
	    }

	    /* don't jog further onto limits */
	    if (!checkJog(joint_num, emcmotCommand->vel)) {
		SET_JOINT_ERROR_FLAG(joint, 1);
		break;
	    }
	    /* set destination of jog */
	    refresh_jog_limits(joint);
	    if (emcmotCommand->vel > 0.0) {
		joint->free_pos_cmd = joint->max_jog_limit;
	    } else {
		joint->free_pos_cmd = joint->min_jog_limit;
	    }
	    /* set velocity of jog */
	    joint->free_vel_lim = fabs(emcmotCommand->vel);
	    /* and let it go */
	    joint->free_tp_enable = 1;
	    /*! \todo FIXME - should we really be clearing errors here? */
	    SET_JOINT_ERROR_FLAG(joint, 0);
	    /* clear axis homed flag(s) if we don't have forward kins.
	       Otherwise, a transition into coordinated mode will incorrectly
	       assume the homed position. Do all if they've all been moved
	       since homing, otherwise just do this one */
	    clearHomes(joint_num);
	    break;

	case EMCMOT_JOG_INCR:
	    /* do an incremental jog */

	    /* check axis range */
	    rtapi_print_msg(RTAPI_MSG_DBG, "JOG_INCR");
	    rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
	    if (joint == 0) {
		break;
	    }

	    /* must be in free mode and enabled */
	    if (GET_MOTION_COORD_FLAG()) {
		reportError("Can't jog axis in coordinated mode.");
		SET_JOINT_ERROR_FLAG(joint, 1);
		break;
	    }
	    if (!GET_MOTION_ENABLE_FLAG()) {
		reportError("Can't jog axis when not enabled.");
		SET_JOINT_ERROR_FLAG(joint, 1);
		break;
	    }

	    /* don't jog further onto limits */
	    if (!checkJog(joint_num, emcmotCommand->vel)) {
		SET_JOINT_ERROR_FLAG(joint, 1);
		break;
	    }
	    /* set target position for jog */
	    if (emcmotCommand->vel > 0.0) {
		joint->free_pos_cmd += emcmotCommand->offset;
	    } else {
		joint->free_pos_cmd -= emcmotCommand->offset;
	    }
	    /* don't jog past limits */
	    refresh_jog_limits(joint);
	    if (joint->free_pos_cmd > joint->max_jog_limit) {
		joint->free_pos_cmd = joint->max_jog_limit;
	    }
	    if (joint->free_pos_cmd < joint->min_jog_limit) {
		joint->free_pos_cmd = joint->min_jog_limit;
	    }
	    /* set velocity of jog */
	    joint->free_vel_lim = fabs(emcmotCommand->vel);
	    /* and let it go */
	    joint->free_tp_enable = 1;
	    SET_JOINT_ERROR_FLAG(joint, 0);
	    /* clear axis homed flag(s) if we don't have forward kins.
	       Otherwise, a transition into coordinated mode will incorrectly
	       assume the homed position. Do all if they've all been moved
	       since homing, otherwise just do this one */
	    clearHomes(joint_num);
	    break;

	case EMCMOT_JOG_ABS:
	    /* do an absolute jog */

	    /* check axis range */
	    rtapi_print_msg(RTAPI_MSG_DBG, "JOG_ABS");
	    rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
	    if (joint == 0) {
		break;
	    }
	    /* must be in free mode and enabled */
	    if (GET_MOTION_COORD_FLAG()) {
		reportError("Can't jog axis in coordinated mode.");
		SET_JOINT_ERROR_FLAG(joint, 1);
		break;
	    }
	    if (!GET_MOTION_ENABLE_FLAG()) {
		reportError("Can't jog axis when not enabled.");
		SET_JOINT_ERROR_FLAG(joint, 1);
		break;
	    }

	    /* don't jog further onto limits */
	    if (!checkJog(joint_num, emcmotCommand->vel)) {
		SET_JOINT_ERROR_FLAG(joint, 1);
		break;
	    }
	    /*! \todo FIXME-- use 'goal' instead */
	    joint->free_pos_cmd = emcmotCommand->offset;
	    /* don't jog past limits */
	    refresh_jog_limits(joint);
	    if (joint->free_pos_cmd > joint->max_jog_limit) {
		joint->free_pos_cmd = joint->max_jog_limit;
	    }
	    if (joint->free_pos_cmd < joint->min_jog_limit) {
		joint->free_pos_cmd = joint->min_jog_limit;
	    }
	    /* set velocity of jog */
	    joint->free_vel_lim = fabs(emcmotCommand->vel);
	    /* and let it go */
	    joint->free_tp_enable = 1;
	    SET_JOINT_ERROR_FLAG(joint, 0);
	    /* clear axis homed flag(s) if we don't have forward kins.
	       Otherwise, a transition into coordinated mode will incorrectly
	       assume the homed position. Do all if they've all been moved
	       since homing, otherwise just do this one */
	    clearHomes(joint_num);
	    break;

	case EMCMOT_SET_TERM_COND:
	    /* sets termination condition for motion emcmotDebug->queue */
	    rtapi_print_msg(RTAPI_MSG_DBG, "SET_TERM_COND");
	    tpSetTermCond(&emcmotDebug->queue, emcmotCommand->termCond, emcmotCommand->tolerance);
	    break;

        case EMCMOT_SET_SPINDLESYNC:
            rtapi_print_msg(RTAPI_MSG_DBG, "SET_SYNCHRONIZATION");
            tpSetSpindleSync(&emcmotDebug->queue, emcmotCommand->spindlesync);
            break;

	case EMCMOT_SET_LINE:
	    /* emcmotDebug->queue up a linear move */
	    /* requires coordinated mode, enable off, not on limits */
	    rtapi_print_msg(RTAPI_MSG_DBG, "SET_LINE");
	    if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG()) {
		reportError
		    ("need to be enabled, in coord mode for linear move");
		emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
		SET_MOTION_ERROR_FLAG(1);
		break;
	    } else if (!inRange(emcmotCommand->pos)) {
		reportError("linear move %d out of range", emcmotCommand->id);
		emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
		tpAbort(&emcmotDebug->queue);
		SET_MOTION_ERROR_FLAG(1);
		break;
	    } else if (!checkLimits()) {
		reportError("can't do linear move with limits exceeded");
		emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
		tpAbort(&emcmotDebug->queue);
		SET_MOTION_ERROR_FLAG(1);
		break;
	    }

	    /* append it to the emcmotDebug->queue */
	    tpSetId(&emcmotDebug->queue, emcmotCommand->id);
	    rtapi_print_msg(RTAPI_MSG_DBG, "SET_LINE");
	    if (-1 == tpAddLine(&emcmotDebug->queue, emcmotCommand->pos, emcmotCommand->motion_type)) {
		reportError("can't add linear move");
		emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
		tpAbort(&emcmotDebug->queue);
		SET_MOTION_ERROR_FLAG(1);