control.c

CNC 的开放码,EMC2 V2.2.8版

		/* This state is called when the machine has determined the
		   switch position as accurately as possible.  It sets the
		   current joint position to 'home_offset', which is the
		   location of the home switch in joint coordinates. */
		/* set the current position to 'home_offset' */
		offset = joint->home_offset - joint->pos_fb;
		/* this moves the internal position but does not affect the
		   motor position */
		joint->pos_cmd += offset;
		joint->pos_fb += offset;
		joint->free_pos_cmd += offset;
		joint->motor_offset -= offset;
		/* next state */
		joint->home_state = HOME_FINAL_MOVE_START;
		immediate_state = 1;
		break;

	    case HOME_INDEX_ONLY_START:
		/* This state is used if the machine has been pre-positioned
		   near the home position, and simply needs to find the
		   next index pulse.  It starts a move at latch_vel, and
		   sets index-enable, which tells the encoder driver to
		   reset its counter to zero and clear the enable when the
		   next index pulse arrives. */
		/* is the joint already moving? */
		if (joint->free_tp_active) {
		    /* yes, reset delay, wait until joint stops */
		    joint->home_pause_timer = 0;
		    break;
		}
		/* has delay timed out? */
		if (joint->home_pause_timer < (HOME_DELAY * servo_freq)) {
		    /* no, update timer and wait some more */
		    joint->home_pause_timer++;
		    break;
		}
		joint->home_pause_timer = 0;
		/* Although we don't know the exact home position yet, we
		   we reset the joint coordinates now so that screw error
		   comp will be appropriate for this portion of the screw
		   (previously we didn't know where we were at all). */
		/* set the current position to 'home_offset' */
		offset = joint->home_offset - joint->pos_fb;
		/* this moves the internal position but does not affect the
		   motor position */
		joint->pos_cmd += offset;
		joint->pos_fb += offset;
		joint->free_pos_cmd += offset;
		joint->motor_offset -= offset;
		/* set the index enable */
		joint->index_enable = 1;
		/* set up a move at 'latch_vel' to find the index pulse */
		home_start_move(joint, joint->home_latch_vel);
		/* next state */
		joint->home_state = HOME_INDEX_SEARCH_WAIT;
		break;

	    case HOME_INDEX_SEARCH_START:
		/* This state is called after the machine has made a low
		   speed pass to determine the limit switch location. It
		   sets index-enable, which tells the encoder driver to
		   reset its counter to zero and clear the enable when the
		   next index pulse arrives. */
		/* set the index enable */
		joint->index_enable = 1;
		/* and move right into the waiting state */
		joint->home_state = HOME_INDEX_SEARCH_WAIT;
		immediate_state = 1;
		home_do_moving_checks(joint);
		break;

	    case HOME_INDEX_SEARCH_WAIT:
		/* This state is called after the machine has found the
		   home switch and "armed" the encoder counter to reset on
		   the next index pulse. It continues at low speed until
		   an index pulse is detected, at which point it sets the
		   final home position.  If the move ends or hits a limit
		   before an index pulse occurs, the home is aborted. */
		/* has an index pulse arrived yet? encoder driver clears
		   enable when it does */
		if ( joint->index_enable == 0 ) {
		    /* yes, stop motion */
		    joint->free_tp_enable = 0;
		    /* go to next step */
		    joint->home_state = HOME_SET_INDEX_POSITION;
		    immediate_state = 1;
		    break;
		}
		home_do_moving_checks(joint);
		break;

	    case HOME_SET_INDEX_POSITION:
		/* This state is called when the encoder has been reset at
		   the index pulse position.  It sets the current joint 
		   position to 'home_offset', which is the location of the
		   index pulse in joint coordinates. */
		/* set the current position to 'home_offset' */
		joint->motor_offset = -joint->home_offset;
		joint->pos_fb = joint->motor_pos_fb -
		    (joint->backlash_filt + joint->motor_offset);
		joint->pos_cmd = joint->pos_fb;
		joint->free_pos_cmd = joint->pos_fb;
		/* next state */
		joint->home_state = HOME_FINAL_MOVE_START;
		immediate_state = 1;
		break;

	    case HOME_FINAL_MOVE_START:
		/* This state is called once the joint coordinate system is
		   set properly.  It moves to the actual 'home' position,
		   which is not neccessarily the position of the home switch
		   or index pulse. */
		/* is the joint already moving? */
		if (joint->free_tp_active) {
		    /* yes, reset delay, wait until joint stops */
		    joint->home_pause_timer = 0;
		    break;
		}
		/* has delay timed out? */
		if (joint->home_pause_timer < (HOME_DELAY * servo_freq)) {
		    /* no, update timer and wait some more */
		    joint->home_pause_timer++;
		    break;
		}
		joint->home_pause_timer = 0;
		/* plan a move to home position */
		joint->free_pos_cmd = joint->home;
		/* do the move at max speed */
		/*! \todo FIXME - should this be search_vel? or another user
		   specified speed? or is a rapid OK? */
		joint->free_vel_lim = joint->vel_limit;
		/* start the move */
		joint->free_tp_enable = 1;
		joint->home_state = HOME_FINAL_MOVE_WAIT;
		break;

	    case HOME_FINAL_MOVE_WAIT:
		/* This state is called while the machine makes its final
		   move to the home position.  It terminates when the machine 
		   arrives at the final location. If the move hits a limit
		   before it arrives, the home is aborted. */
		/* have we arrived (and stopped) at home? */
		if (!joint->free_tp_active) {
		    /* yes, stop motion */
		    joint->free_tp_enable = 0;
		    /* we're finally done */
		    joint->home_state = HOME_FINISHED;
		    immediate_state = 1;
		    break;
		}
		if (joint->on_pos_limit || joint->on_neg_limit) {
		    /* on limit, check to see if we should trip */
		    if (!(joint->home_flags & HOME_IGNORE_LIMITS)) {
			/* not ignoring limits, time to quit */
			reportError("hit limit in home state %d",
			    joint->home_state);
			joint->home_state = HOME_ABORT;
			immediate_state = 1;
			break;
		    }
		}
		break;

	    case HOME_FINISHED:
		SET_JOINT_HOMING_FLAG(joint, 0);
		SET_JOINT_HOMED_FLAG(joint, 1);
		SET_JOINT_AT_HOME_FLAG(joint, 1);
		joint->home_state = HOME_IDLE;
		immediate_state = 1;
		break;

	    case HOME_ABORT:
		SET_JOINT_HOMING_FLAG(joint, 0);
		SET_JOINT_HOMED_FLAG(joint, 0);
		SET_JOINT_AT_HOME_FLAG(joint, 0);
		joint->free_tp_enable = 0;
		joint->home_state = HOME_IDLE;
		joint->index_enable = 0;
		immediate_state = 1;
		break;

	    default:
		/* should never get here */
		reportError("unknown state '%d' during homing",
		    joint->home_state);
		joint->home_state = EMCMOT_ABORT;
		immediate_state = 1;
		break;
	    }	/* end of switch(joint->home_state) */
	} while (immediate_state);
    }	/* end of loop through all joints */

    if ( homing_flag ) {
	/* at least one axis is homing, set global flag */
	emcmotStatus->homing_active = 1;
    } else {
	/* is a homing sequence in progress? */
	if (emcmotStatus->homingSequenceState == HOME_SEQUENCE_IDLE) {
	    /* no, single axis only, we're done */
	    emcmotStatus->homing_active = 0;
	}
    }
}

static void get_pos_cmds(long period)
{
    int joint_num, all_homed, all_at_home, result;
    emcmot_joint_t *joint;
    double positions[EMCMOT_MAX_JOINTS];
/*! \todo Another #if 0 */
#if 0
    static int interpolationCounter = 0;
#endif
    double old_pos_cmd;
    double max_dv, tiny_dp, pos_err, vel_req, vel_lim;

    /* used in teleop mode to compute the max accell requested */
    double accell_mag;
    int onlimit;

    /* RUN MOTION CALCULATIONS: */

    /* run traj planner code depending on the state */
    switch ( emcmotStatus->motion_state) {
    case EMCMOT_MOTION_FREE:
	/* in free mode, each joint is planned independently */
	/* Each joint has a very simple "trajectory planner".  If the planner
	   is enabled (free_tp_enable), then it moves toward free_pos_cmd at
	   free_vel_lim, obeying the joint's accel and velocity limits, and
	   stopping when it gets there.  If it is not enabled, it stops as
	   quickly as possible, again obeying the accel limit.  When
	   disabled, free_pos_cmd is set to the current position. */
	/* initial value for flag, if needed it will be cleared below */
	SET_MOTION_INPOS_FLAG(1);
	for (joint_num = 0; joint_num < num_joints; joint_num++) {
	    /* point to joint struct */
	    joint = &joints[joint_num];
	    //AJ: only need to worry about free mode if joint is active
	    if (GET_JOINT_ACTIVE_FLAG(joint)) {
		joint->free_tp_active = 0;
		/* compute max change in velocity per servo period */
		max_dv = joint->acc_limit * servo_period;
		/* compute a tiny position range, to be treated as zero */
		tiny_dp = max_dv * servo_period * 0.001;
		/* calculate desired velocity */
		if (joint->free_tp_enable) {
		    /* planner enabled, request a velocity that tends to drive
		       pos_err to zero, but allows for stopping without position
		       overshoot */
		    pos_err = joint->free_pos_cmd - joint->pos_cmd;
		    /* positive and negative errors require some sign flipping to
		       avoid sqrt(negative) */
		    if (pos_err > tiny_dp) {
			vel_req =
			    -max_dv + sqrt(2.0 * joint->acc_limit * pos_err +
			    max_dv * max_dv);
			/* mark joint active */
			joint->free_tp_active = 1;
		    } else if (pos_err < -tiny_dp) {
			vel_req =
			    max_dv - sqrt(-2.0 * joint->acc_limit * pos_err +
			    max_dv * max_dv);
			/* mark joint active */
			joint->free_tp_active = 1;
		    } else {
			/* within 'tiny_dp' of desired pos, no need to move */
			vel_req = 0.0;
		    }
		} else {
		    /* planner disabled, request zero velocity */
		    vel_req = 0.0;
		    /* and set command to present position to avoid movement when
		       next enabled */
		    joint->free_pos_cmd = joint->pos_cmd;
		}
		/* if we move at all, clear AT_HOME flag */
		if (joint->free_tp_active) {
		    SET_JOINT_AT_HOME_FLAG(joint, 0);
		}
                if ( joint->home_state == HOME_IDLE ) {
                    /* velocity limit = planner limit * global scale factor */
                    /* the global factor is used for feedrate override */
                    vel_lim = joint->free_vel_lim * emcmotStatus->net_feed_scale;
                } else {
                    /* except if homing, when we ignore FO */
                    vel_lim = joint->free_vel_lim;
                }
		/* must not be greater than the joint physical limit */
		if (vel_lim > joint->vel_limit) {
		    vel_lim = joint->vel_limit;
		}
		/* limit velocity request */
		if (vel_req > vel_lim) {
		    vel_req = vel_lim;
		} else if (vel_req < -vel_lim) {
		    vel_req = -vel_lim;
		}
		/* ramp velocity toward request at axis accel limit */
		if (vel_req > joint->vel_cmd + max_dv) {
		    joint->vel_cmd += max_dv;
		} else if (vel_req < joint->vel_cmd - max_dv) {
		    joint->vel_cmd -= max_dv;
		} else {
		    joint->vel_cmd = vel_req;
		}
		/* check for still moving */
		if (joint->vel_cmd != 0.0) {
		    /* yes, mark joint active */
		    joint->free_tp_active = 1;
		}
		/* integrate velocity to get new position */
		joint->pos_cmd += joint->vel_cmd * servo_period;
		/* copy to coarse_pos */
		joint->coarse_pos = joint->pos_cmd;
		/* update joint status flag and overall status flag */
		if ( joint->free_tp_active ) {
		    /* active TP means we're moving, so not in position */
		    SET_JOINT_INPOS_FLAG(joint, 0);
		    SET_MOTION_INPOS_FLAG(0);
		    /* is any limit disabled for this move? */
		    if ( emcmotStatus->overrideLimitMask ) {
			emcmotDebug->overriding = 1;
		    }
		} else {
		    SET_JOINT_INPOS_FLAG(joint, 1);
		    /* joint has stopped, so any outstanding jogs are done */
		    joint->kb_jog_active = 0;
		    joint->wheel_jog_active = 0;
		}
	    }//if (GET_JOINT_ACTIVE_FLAG(join))
	}//for loop for joints
	/* if overriding is true and we're in position, the jog
	   is complete, and the limits should be re-enabled */
	if ( (emcmotDebug->overriding ) && ( GET_MOTION_INPOS_FLAG() ) ) {
	    emcmotStatus->overrideLimitMask = 0;
	    emcmotDebug->overriding = 0;
	}
	/*! \todo FIXME - this should run at the traj rate */
	all_homed = 1;
	all_at_home = 1;
	/* copy joint position feedback to local array */
	for (joint_num = 0; joint_num < num_joints; joint_num++) {
	    /* point to joint struct */
	    joint = &joints[joint_num];
	    /* copy coarse command */
	    positions[joint_num] = joint->coarse_pos;
	    /* check for homed */
	    if (!GET_JOINT_HOMED_FLAG(joint)) {
		all_homed = 0;
		all_at_home = 0;
	    } else if (!GET_JOINT_AT_HOME_FLAG(joint)) {
		all_at_home = 0;
	    }
	}
	/* if less than a full complement of joints, zero out the rest */
	while ( joint_num < EMCMOT_MAX_JOINTS ) {
	    positions[joint_num++] = 0.0;
	}
	switch (kinType) {

	case KINEMATICS_IDENTITY:
	    kinematicsForward(positions, &emcmotStatus->carte_pos_cmd, &fflags, &iflags);
	    if (checkAllHomed()) {
		emcmotStatus->carte_pos_cmd_ok = 1;
	    } else {
		emcmotStatus->carte_pos_cmd_ok = 0;
	    }
	    break;

	case KINEMATICS_BOTH:
	    if (checkAllHomed()) {
		/* is previous value suitable for use as initial guess? */
		if (!emcmotStatus->carte_pos_cmd_ok) {
		    /* no, use home position as initial guess */
		    emcmotStatus->carte_pos_cmd = emcmotStatus->world_home;
		}
		/* calculate Cartesean position command from joint coarse pos cmd */
		result =
		    kinematicsForward(positions, &emcmotStatus->carte_pos_